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The purpose of the Kalman filter is to keep track of the state of the 
system by means of a sequence of noisy measurements. Conceptually, this 
rechnique may be viewed as 2 methoc t3 systematicaily recuce We 
measurement errars associated with the observations of tne {targaes 
acoustic ignature to determine an optimal estimate of the tardaets 
position and motion. 


When bath tne system and measurement models are linear tunctiane of 


dket) = }{k)- vk) + TO) wk) ame 
and the measurement equation 1s related to the state by 
2({k} = H(k)-x(k) + Ak) 


poe 

CA 
| 
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where x(k) is the N x | dimensianal state vector, 
d(k) is the Nx N- dimensional transition matrix, 
wik) is the Mx 1 dimensional vector of random forcing 
functions 
rie) ig the Nx M dimensional state forcing matrix, 


= 
fy pe sxe ne , : ; a ee 
wieiig tha Hy» ! dimensional measurament vectar, 
te Oe ee es = ~ rl outa 
Kiki isg the Pw mn dimensional measurement matrix 


Wikis tie 9% se sci cinemas cee an 
hy 


” +r - ~ of, ee Po, 
nonimear Measurement Mager Becomes 


where the measurement, Z(K), is a function, Acetk) K) af the etate variables 


t 


; ~ -, ¥ nA : ele mo “VaR imam Tinga 
mimereen = foise 1)e error), WR) The Ale(t)k)} must be “linearizeq.” This 
If accomplished Dy expanding h ina Taylor series about the Gesi estiniat¢ 


eeeewet aie af ine time and ons tne first order terms are bent (Ret. 
$:p.34] and (Ref. 2:p.1982], The application of the Kaiman filter ta the 
nonlinear case is called the Extended Kalman filter. Higher order, more 
precise approximations to the optimal nonlinear filter can be acheived 
using more terms of the Taylor Series expansion for the nonlinearities, 
and DY derviving recursive relations for the higher moments of state 
Variables. For a derivation and additional discussion of Extended Kaimarn 
Tiltering tne reader can refer to (Ref. Z2:pp. 160-225] or [Ref 6:pp. 219- 
née Tolhowing will be a summarized discussion of the discrets 


Extended Kaiman Tiler equations. The linear form of equation (3-2) yields 


Z(k) = BO) extk) + vk) coe) 

where Hik) = OntxGek) | 
ei . i ero 7 = pee 
920) ee |) (oor 


r-1} 1S the N-dimensional predicted values of the state vector Defers 
Ine kth measurement. That 15 


x(k [K-1) = @(k)-x(k-1[k-1) (3-4) 
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Vari ance ACK 
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ts Nona ee . 
E[vlk vip") = (0, kay 


R(k), k=] (Saaly 
= A(k)d,, for all k,j 20. 


2. The random forcing function Is Zero mean and uncorrelated with 
covariance O'(k) 

E{wik}]) = D. - all kx O (3=ban 

Flws(k }- wi i)F = fj'{kJ- Spalomall kageaes (SG 


_ The random forcing input and measurement noise are uncorrelate 
Elw(k yt = Ely )-wi) = 0 for al! ky oa 
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fate Is a random variable with known mean and 22- 


Siege), Ho | (76 
ang 
E(x(0) - doh fx(O) - IT} = PlO]- = Py 2-9) 
>. ihe initial state and measurernent be are uncorrelated 
Efa(O)-w(k) 4) = Elvtk)-x(0)"} =O for allkyO. (3-10) 


6. The random a input and initial state are uncorrelated 
Fiw(kyx(O)"] = E[(O)w(k)] =O for allkyO. (3-14) 


The state estimation error vector x(k) is define by the estimated state 


vector minus the true state vector 
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mk | = th ik) xfk) me 
and [rhe aregicted state estimation errar vector is cefined Du ime predicte7 
stale vector minus ine true state vector 

ait [eo 1) = xk] keo1) > xfk (3-13) 
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. moe xy , 
Say eee ea ’ = Su a ie es 
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gG Ihe predicted covariance of state errar matrix cz 

es = E{x(k | k-1)xtk Ek- 1 (3-15) 
The state excitation covariance matrix is Given BY 
Ti (3-16) 
rk) Ok) (Kk)! 
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imate is selected to have the form 
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if the estimation equation (3-17) is initialized with the value 


ACO |-1) = x5. (3-25) 
it car be shown that the optimal estimate x(k |k) is unbiased i.e. 
E{tx(k [k= xlk)]} = 0 for all ky 0. (3-22) 


and the initial condition is 


in summaru, Table 3-1 defines the discrete Extended Kalman filter al- 


ar state equation (2-103 and the linearized measure- 
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QUr lth for the din 
ment equation (3-3). Equations (3-17), (5-18, (3-19), ($-4). an 
camprise the Extended Kalman fiiter recursive equations.jRef, 2:6. 130) 
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are shown in Figure (3-1). 


Table 3-1 SUMMARY OF KALMAN EQUATIONS 


INITLAL CONDITIONS: 
As SUPP AT TONS: 


GAIN EQUATION: 
lk) = Pe La Pra Me THER) Ptk eT re) +RCk Pt 


FRROR COVARIANCE UPDATE EQUATION: 


P(k [k} = [I-G(k)- HOT P (kK { k-1) 
OTA es ESTIMATE UPDATE EQUATION: 


a 


oe Sk [K-1) + GU) 1Z(k) - WOMk | k= 


i ANCE PROPAGATION EQUATION: 
= Bk) PCR LK) BK) + O(k) 
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Linen equations and ine sequence oF Computationai sié 





Enter prior estimate x(k|k-1) and 
its error covariance P(k|k-1) 


Evaluate H(k)= dh(x(k),k) 
Ox(k) x(k) 2 x(k | k-1) 

Compute the Kalman gain: ; 5 

G(k) = P(k | k-1) H(k)) [H(k)P(k | K- 1) H(k) + R(K)I 















Update estimate with measurement z(k) : 
R(k[K) = RK] K-1) + G(KZ(K)-NCR(K | K- 1) 






Compute error covariance update : 
P(k]k) = [1 - G(k)H(k)IP(K | k-1) 







Project ahead: 
x(k+1[k) =0(k)x(k | k) 


P(k+ 11k) = O(k)P(k | k)@(k)'+Q(k) 


Increase k by 1 and continue until 
Stopping point is reached 
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Figura s-] Kalman Fiiter Hocureive Laon 
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falman filter to the passive acoustic tracking probiern will be ciscussed. 


Given the system anda noise-free measurement models developed in Section 
I], we will derive the random forcing function w(k), the state excitation 
covariance matrix Q(k), the measurement equation 2(k), and the 


measurement AGise Covariance matrix R(k). 





From eguation (2-19) the vector w(k) represents the effect am the 
states af the random forcing tunction and may be calculated from }equa- 
trons relating v,, and vy, to tne target neading, G,, and velocity, Vv, From 


rigure (2-1) the velocity in the « direction ts 
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The random forcing tunctians covariance matrix 1s 
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The variance of watk) and walk) can be found in similar fashion to dive 
Qe ee eo 2 eae (>t 
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Upon substituting for wk), and wok), and simplifying the covariance term 
c[wiik }-walk J} Decomes 
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Substituting equations (2-21) through (2-25) into equation (2-13) the O'(k) 
matriy result 1s 


ot pT oe G 


V xy 
“4 t f , mes road feo ™ _ —_ 
Ove} = Elwike wiki} = ee one Q [> oe 
a cy ee 
v Fp 


au Cant: Qo eNes 2 reek aS 
se ee “oy és 


~” “ “ = rr ~ a ™ ~. : > ; 
aimee are evaluated at tne predicted values GI Vv, ang v7, 


. oft 


vn et 
. 


Excitation Covariance Matrue 
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To compute the error covariance propagation equation (3-19) th 


State excitation covariance matrix Q(k) must be Known. The size af Otk; 
Nas a direct bearing on the magnitude of the P(k+1/k) [Ref. 2:p. 76]. The 


POSSIDILity Of a maneuvering target and model inaccuracies are taken Into 
account Dy tne state excitation covariance matrix. As more anc mare data 


is Processed, Off) prevents the Kalman gains from approaching Zero Dy 
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INSUrING same uncertainty in the or 
(3-16), the state excitation covariance matrix 1s 
Ce =e eo rk. 6-15) 

Substituting from equation (2-9) for F(k) and (3-38) for Q'(k) the state 


excitation covariance matrix | 
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From equation (3-2) the nonlinear measurement equation is 
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As indicated in equation (3-3a) and (3-3b) the linear form of this equation 
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Similarly, the partial derivatives af the , angie measure- 


ment evaluated at tne predicted state values x(k |k-1) follows: 
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where OO, Is the standard deviation of the frequency measurement noise 


Oa 1S the standard deviation of the bearing measurement noise 
AS indicated py Mitschang (Ref. 3:p. 43], tne resolution of the 
Traquency measurement 1s ecual ta the inverse of the record lengin of the 
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error covariance. In otherwords when the calculated covariance matrt. 
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state diverge because the system model in the fiiter is citferant than tne 
actual system model. Such madel errors are due to the following: 


i. Approximations that might be made to simplify the tilter 
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Limited knowledge of the physical system. 

Computational errors resulting from the use of finite precision 

ar linmetic 
im order to prevent divergence and to accommodate maneuvering, th 
pasic idea is to increase the calculated covariance matrix, Pk ik); since 
mode! errors are compensated by a larger calcuiated covariance matri. 
payever, this increase in the calculated error covariance makes the friter 


more sensitive tO random errors in the measurement cracess, oftas 


merease the calculated error covariance oniy when the targer as 
vaneuvered. Further details on divergence is delineated in By Jazwinsk i 
[Ref.7:pp. 301-305! and Gelb (Ref. 2:pp. 277-311]. 

The first pracedure used to prevent divergence and to allow for ma- 
neuvering is the random forcing function wk), As indicated in Subsac- 
tion HLC.2, Qk} prevents the Kalman gains from approaching zerc Du 
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nerefore the predicted residual stanaarad deviation 1s defined as 
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in order to allow for target maneuvers, we devine an adaptive gate as 
three times the predicited residual standard deviation 
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ne constants in equation (5-55) were found by trial and error, 
random forcing function covariance matrix increases, which increases 
Q(ky. When the. state excitation matrix, Q(k), increases, the covariance 
matrix, P(kik-i) increases. The covariance matrix causes the adaptive 
gate to increase. Note H(k} and R(k) remain the same. Hence. the aate 
opens the Tiiter’to the incoming measurement. At the next iteration the 
variances of the random rorcing function reverts back to eguation (3-54) 
ro calculate random forcing function covariance Matrix and the state 
exicitalion covariance matrix for the next measurement. 

if [he predicted residual exceeds the adaptive agate three consecutive 
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crror ellipsoids are useful in visualizing the estimation error. witn 
them we can consider the true state value to lie within @ certain region 
surrounding the estimate .. This uncertainy 1s expressed in the covariance 
of error matrix P(k{k). The concept of the error ellipsoid is summarized. 


DEFINITION, Suppose the n-dimensional vector random variabdie x has a 


multivariate gaussian ears ion with a mean value of Zero and covar- 
ance —|<ee” is ‘error ellipsoids” are defineg as a-dimensiona! sur 
faces af constant probability densi ty, (Ref, &:p, 252] 
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The estimate, x(k jk), which is a linear combination of x(k) and vik). 
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3.The estirnate error, x(k) = xk |k) - x(k) 


As indicated in Section ili, the state estimation error is Zero mean with 
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Figure 6-1. Scenario 1: Geographic Plot- 
Noise, Q'(k), and Adaptive Control Applied 
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Figuré 6-2. Scenario 1-Simulation 1: 
Enlarged Geograpnic Plot- Q’'xk) Applied 
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Figure 6-3. Scenario |-Simulation 2: Enlarged Geographic Plot- 
Q'AK) and Adaptive Control Applied 
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Figure 6-4. Scenario 1-Simulation 3: 
Enlarged Geographic Plot- Q'(k) Applied 
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Figure 6-5. Scenario 1-Simulation 4: Enlarged Geograpnic Plot- 
Q'(k) and Adaptive Control Applied 
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Figure 6-6. Scenario |-Simulation S:Enlarged Geographic Plot- 
Noise and Q’{k) Applied 
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Figure 6-7. Scenario 1-Simulation 6:Enlarged Geographic Plot- 
Noise, Q'AK), and Adaptive Control Applied 
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Figure 6-8. Scenario 1-Simulation 7:Enlarged Geographic Plot- 
Noise and Q'(k) Applied 
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Figure 6-9. Scenario |-Simulation 8:Enlarged Geographic Plot- 
Noise, Q'(k), and Adaptive Control Applied. 
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exceeded the adaptive gate at that particular time. Figure (6-10) agrees 
with Table (6-7) information. We can see in Figure (6-10} that during the 
30 deg turn at time t, = 12 and t, = 13 the adaptive gate increases to 
admit the frequency measurement from buoy | to the filter. Similarly, 


the adaptive gate increases during the large 360 deg and small 360 deq 
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Figure 6-10. Scenario 1-Simulation &: 
Frequency Measurement-Predicted Residual and Adaptive Gate 
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Figure 6-11. Scenario 1-Simulation 8: 
Bearing Measurement-Predicted Residual and Adaptive Gate 


Figures (6-12) and (6-13) show the variances of the position 
components, x component is P,(k|k) and y component is P3s(k|k), from 
the error covariance matrix for the frequency and bearing measurement 
respectively. Both figures show the x and y component variances de- 
creasing rapidly. This 1s due to the a prior! position information, op 


7) 


equals 0.5 nm (1012 yds). Hence the variance 0,2 is approximately 10° 
yds*. Figures (6-14) and (6-15) are the result of expanding Figures (6-12) 
and (6-13). In these graphs we can see how the position variances change 
during each of the target's maneuvers. Note that the frequency and bear- 


ing measurement position variances are nearly the same for each buoy. 
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Figure 6-12. Scenario 1-Simulation 8: 
Frequency Measurement-Position Variances 
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Figure 6-13. Scenario 1-Simulation 8: 
Bearing Measurement-Position Variances 
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Figure 6-14. Scenario 1-Simulation 8: Expanded View 
of the Frequency Measurement Position Variances 
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Figure 6-15. Scenario 1-Simulation 8: Expanded View 
of the Bearing Measurement Position Variances 
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cormiporient error 1s greatest during this time(from t, = 52 to t, 225 mings 
the error is qreater than 100 uds). 
Figures (6-19) and (6-20) illustrates the change in the velocity 
and v= Pagk{k), for simulation 8. Note that tne 
'requency and bearing measurernent velocitu variances are veru similar. 
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Figure 6-16 Scenario |-Simulation 8: Buoy 1's 
Position Variances and Experimental Variances 
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Figure 6-17. Scenario 1-Simulation 8: Buoy 2's 
Position Variances and Experimental Variances 
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Figure 6-18. Scenario 1-Simulation 8: Buoy 3's 
Position Variances and Experimental Variances 
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Figure 6-19. Scenario 1-Simulation 8: 
Frequency Measurement-Velocity Variances 
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Figure 6-20. Scenario !-Simulation 8: 
Bearing Measurement-Velocity Variances 
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The frequency variance for the frequency ard Dearing measurements 
lustrated in Figures (6-21) and (6-22). Like the velocity variances, 


frequency variance for the bearing measurement is very similar to the 


frequency variance for frequency measurement. 
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Figure 6-21 Scenario |1-Simulation 3: 
Frequency Measurement-Frequency Variances 
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Figure 6-22. Scenario 1-Simulation 9: 
Bearing Measurement-Frequency Variances 
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Figures (6-23)-(6-28) illustrates tne position, velocity and fre- 
quency components Kalman gains for the frequency and bearing measure- 
ments. In Figures (6-23) and (6-24) tne position gains decrease towards 
zero with small deviations for the turns. The velocity and frequency 


gains, Figures (6-25) - (6-28) are very erratic during the turns. 
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Figure 6-23. Scenario !-Simulation 8: 
Frequency Measurement - Kalman Gains for Position Components 
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We can now explain why tne Dearing measurement’s variances are 
similar to the fresuency measurement’s variances. Recall the error covar- 
lance update equation (3-19) from Table 3-1 is 

P(k |k) = (1 - G(k)-H(kK)) - P(k [ k-1) 


and that in this case P(k|k-1) is really the frequency measurement’s 


covariance of error matrix. In Subsection V.D.2 it was shown that 
Pa(k | K-1) = Pk | k). 
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rigure 6-24. Scenario 1-Simulation 8: 
Bearing Measurement - Kalman Gains for Position Components 
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From equations (3-46b), (3-46d), and (3-46e), the partial derivatives of 
the bearing measurement with respect to the velocity and frequency 
components are zero. Hence, only the position components of H(k) are used 
in the calculation of G(k):H(k). It can be seen in figures (6-23)-(6-28), 
that the Kalman gains are not always small numbers. But, examining the 
output data indicates that the position components of H(k) are very small 
compared to G(k), so G(k):H(k) is be small. Therefore the error covariance 
matrix for the bearing measurement P(k | k) will be approximately equal to 


the error covariance of the frequency measurement P(k | k-1). 
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Figure 6-25. Scenario |-Simulation 8: 
Frequency Measurement - Kalman Gains for Velocity Components 
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Figure 6-26. Scenario 1-Simulation 8: 
Bearing Measurement - Kalman Gains for Velocity Components 
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Figure 6-27. Scenario |1-Simulation 8: 
Frequency Measurement - Kalman Gain for Frequency Component 
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Figure 6-28. Scenario 1-Simulation 8: 
Bearing Measurement - Kalman Gain for Frequency Component 
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C. SCENARIO 2 

scenario 2 is a continuation of Scenario !-Simulation 8. The last 
state estimate x(59|59) and last error covariance matrix P(S9|59) from 
Simulation 8 is used to initialize Scenario 2. DIFAR 3 from Scenario | is 
dropped and another DIFAR 3 is placed west of the target’s track as illus- 
trated in figure (6-29). An enlarged geographic plot is shown in Figure 
(6-30). Table (6-8) lists the maximum position error for each maneuver. 
The left hand column of ‘Table (6-30) is taken from Table (S-1). The 
filter's estimated track accurately reconstructs the actual track. As 
shown in figure (6-30) and Table (6-8) the maximum errors occur during 


the last segment of both “s” turns. 
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Figure 6-29. Scenario 2: GeographicPlot- 
Noise, Q'(k), and Adaptive Control Applied 
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Figure 6-30 Scenario 2: Enlarged Geograpnic Plot- 
Noise, Q'(k), and Adaptive Control Applied 
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Figure 6-31. Scenario 2: Frequency Measurement- 
Predicted Residual and Adaptive Gate 
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Figure 6-32. Scenario 2: Bearing Measurement- 
Predicted Residual and Adaptive Gate 
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Figure (6-33). The values of the bearing measurement’s positions 
variances are slightly less than the frequency measurement’s position 
variances values. | 

| Figure (6-35) illustrates the velocity components variances for the 
frequency and bearing measurement. As indicated for Scenario |, the 
bearing measurement’s velocity variances are very similar to the fre- 
quency measurement’s velocity variances. Hence, only one graph Is dis- 
played. Note the large value for vy variance for buoy | at t, = 100 mins. 


This is when the target is completing the small “s” maneuver. 
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Figure 6-33. Scenario 2: Frequency Measurement- 
Position Variances 
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Figure 6-34. Scenario 2: Bearing Measurement- 
Position Variances 
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Figure 6-35. Scenario 2: Frequency and Bearing 
Measurement - Velocity Variances 
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The frequency component variance 1s shown in Figure (6-36). Since 
the bearing and frequency measurement’s frequency component variance 1s 


nearly the same, only .one graph Is Shown. 
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Figure 6-36. Scenario 2: Frequency and Bearing 
Measurement-Frequency Variance 
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Figures (6-37)-(6-42) illustrates the Kalman gains for the position, 


velocity and frequecny components. The position gains, Figures (6-37) and 


(6-38), are usually less than + 100, but there are deviations when the 


target maneuvers. Again the velocity and frequency gains, Figures (6-39)- 


(6-42), are very erratic during the maneuvers. 
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Figure 6-37. Scenario 2: Frequency Measurement - 
Kalman Gains for Position Components 
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Figure 6-38. Scenario 2: Bearing Measurement- 
Kalman Gains for Position Components 
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Figure 6-33 Scenario 2: Frequency Measurement- 
Kalman Gains for Velocity Components 
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Figure 6-40. Scenario 2: Bearing Measurement - 
Kalman Gains for Velocity Components 
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Figure 6-41. Scenario 2: Frequency Measurement- 
Kalman Gain for Frequency Component 
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Figure 6-42. Scenario 2-: Bearing Measurement - 
Kalman Gain for Frequency Component 
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D. SCENARIO 3 


scenario 3 is a three sonobuoy scenario, where two of the buoys are 


LOFAR. A geographic plot of the target’s track, sonobuoy pattern and the 


filter’s estimated track are shown in Figure (6-43). An enlarged geo- 


graphic plot is shown in Figure (6-44). The target’s track is the same 
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Figure 5-43. Scenario 3: Geographic Plot- 
Noise, Q'(k), and Adaptive Control Applied 
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as the track in Scenario | and is described in Subsection V.B. The 


algorithm generates the estimates and predictions from the measurements 


in the following order: 


| 


“ 
he 


. Frequency measurement from DIFAR | 


2. Bearing measurement from DIFAR | 


. Frequency measurement from LOFAR 2 


. Frequency measurement from LOFAR 3 
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Figure 6-44 Scenario 3: Enlarged Geographic Plot- 
Noise, Q'(k), and Adaptive Control Applied 
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Figure 6-45. Scenario 3: Frequency and Bearing 
Measurement- Predicted Residual and Adaptive Gate 
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Figure 6-48, Scenario 
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Figure 6-47. Scenario 3: Frequency and Bearing 
Measurement - Velocity Variances 
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Figure 6-48. Scenario 3: Frequency and Bearing 
Measurement-Frequency Variance 
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Figure 5-49. Scenario 3: Frequency and Bearing 
Measurement-Kaiman Gains for Position Components 
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Figure 6-51. Scenario 3: Frequency and Bearing 
Measurement - Kalman Gain for Frequency Component 


c. SCENARIO 4 
Scenario 4.15 a continuation of Scenario 3. Like Scenario 2, the last 
state estimate, s(53]59), and last error covariance matrix, P(539|59), 


from Scenario 3 is used to initialize Scenario 4. As illustrated if Figure 





(6-52), LOFAR 3 from Scenario 3 15 dropped and anotner LOFAR 3 is placed 
west of the target’s track. An enlarged geographic plot 1s shown In Figure 
(6-53). Table (6-10) lists the maximum position error fer gach seqment 


of the targets track. 
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Figure 6-S2. Scenario 4: Geographic Plot- ; 
Noise, Q'(k), and Adaptive Control Applied 
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Noise, Q'(k), and Adaptive Control Applied 
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duay | causes the predicted residual to exceed tne adaptive gate three 


times and tne fitter is reinitialized. DIFAR 1's noisy Dearing measurement 


~ 


for t, = 10! mins is {3 Jeqs greater than tne noise-free measuroment. 
The distance tram the target's actual position to Duay | is 2250 yds. A 
13 deg error at this distance is a very large error. After restarting, the 
Tilter locks en to the target at t, = 108 mins with an error of 80 yds. At 


t, =!11 mins the bearing measurement is 13 deqs less tnan the noise-free 
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Figure 6-34. Scenario 4: Frequency and Bearing 
Measurement- Predicted Residual and Adaptive Gate 
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measurement and tne fiiter s estimated position is scutneast of the actual 
position by 7/70 yas. The filter reinitializes again at t, = 113 mins. 
DIFAR 1's noisy bearing measurement is 10 deqgs greater than ihe noise- 
free measurement and the target is less than 2000 yds from Duoy |. 
After restarting the filter continues to track the target. 

Figure (6-55) illustrates the position variances. Note the two peaks 
at t, =101 mins and t, = 113 mins, this is when the filter is reinitialized. 
From equation (3-57) the position components are reinitialize to (0.5 nm)¢ 


or approximately 10° ydsé¢ . 
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riguré &-35. Scenario 4: Frequency and fearing 
Measurement- Position ‘Variances 
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The velocity variances are snown in Figure (6-56). The v, variance for 
Suoy l att, = 100 is 4.32 = 10° (yds/mins)* . This is at the completion 
of the small “s” maneuver. The velocitu components are reinitialized to 
(3kts)* or approximately 107 (yds/mins)¢ at t, =101 mins and ty = 113 
mins. Figure (6-57) illustrates the frequency variances. Instead of reini- 
tializing the frequency component to | hzé as indicated in equation (3-57) 
the algorithm used 10 hz* for this scenario. The Kalman agains for the 
position, velocity, and frequency components are shown in Figures (6-58) - 


(5-60). 
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Figure 6-S6. Scenario 4: Frequency and Bearing 
Measurement - Velocity Variances 
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Figure 6-57. Scenario 4: Frequency and Bear:ng 
Measurement-Frequency Component 
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Figure 8-58, Scenario 4: Frequency and Bearing 
Measurement-Kalman Gains for Position Compeanents 
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Figure 6-60. Scenario 4: Frequency and Bearing 
Measurement- kalman Gain for Frequency Component 
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Figure 6-01, Scenario S-Simulation |: Geograpnic Plot- 
Noise, Q'(k), and Adaptive Control Applied 
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Figure 6-62 Scenario S-Simulation 2: Geographic Plot- 
Noise, Q’{k), and Adaptive Control Applied 
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¥*XX PURPOSE HX 
PROGRAM COMPUTES THE TARGET TRACK 


HE HE HE HE HEHEHE HE HEHE HEHE HEHE HH HHH MMH MRAM RHH MRK RRHRRKRRRKRRKK 


THE OUTPUT IS IN FILE DEVICE 3 


HE HE HE HE HE HE HE HE HEHE HE HE HE HE HE HE HE HE HE HE HE HE HE HE HE HE HE HE HE HE HE HE HH HM HHH RRR 


%¥% VARIABLE DEFINITIONS x 


DT 
HDG 


Il 
ITIME 


PT180 


TACC 
TRATE 


TURN 
VEL 
VX 
VY 


SAMPLE INTERVAL IN SECS. 

MATRIX OF TARGET'S HEADINGS 

COUNTER 

I-l 

INTERVAL TO TIME TURNS ITIME =12, 

EX. ITIME X DT X TRATE = DEGREES OF TURN 
12 X 15 SEC X 0.5 DEG/SEC = 90 DEGS 

COUNTER 

PI X 180 DEGREES 


~ TIME USED IN SECONDS AND CONVERTED TO MINUTES FOR 


OUTPUT 

HORIZ ACCELERATION- YARDS/SEC#¥2 

TURN RATE-INPUT IN DEG/SEC, CONVERTED TO RAD/SEC 
+ CLOCKWISE,- COUNTER CLOCKWISE 

SUBROUTINE TO CALCULATE TURNS 

MATRIX OF TARGET'S VELOCITIES-KNOTS AND YARDS/SEC 
X COMPONENT OF VEL 

Y COMPONENT OF VEL 

X COMPONENT-USED IN NM AND YDS 

Y COMPONENT-USED IN NM AND YDS 


*¥¥X VARIABLE DECLARATIONS 2x 
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QOnaa on 


REAL HDG 


DIMENSION 1(2000),VEL(2000), TRATEC2000),X(2000),YC€2000),HDGC 2000) 


INITIALIZE TARGET DATA 
PI180=0.0174535 

X€1)=10. 

YCL)=le2. 

VELC1)=5.0 

HDG(1)=180.0 

TRATEC1)=0.5 

DT=15. 

TACC=2. 

TO1)20. 

ITIME=15 

CONVERT DIST TO YARDS 

X€1)=X€1)%2025.3667 

YC1)=Y€1)*2025.3667 

CONVERT TURN RATE FROM DEG/SEC TO RAD/SEC 

TRATEC1)=TRATEC1) ¥PI180 

CONVERT VEL FROM KTS TO YARDS/SEC 

VELC1)=VEL C1) ¥0 .562605 

CONVERT HEADING TO RAD/SEC 

HDGC 1) =HDGC1)*PI180 

CONVERT HORIZ ACC FROM YARDS/SEC®¥2 TO YARDS/MIN¥2 
TDATAC8)=TDATAC8)¥35600.0 
WRITEC6,1000) T,X,Y,VEL,HDG 
WRITEC3,1000) T,X,Y,VEL,HDG 
T=] 

IFCTCI).EQ.600.) THEN 

ITIME=12 90 DEG AT 0.5 DEG/SEC 
ITIME=24 180 DEG AT 0.5 DEG/SEC 
ITIME=48 360 DEG AT 0.5 DEG/SEC 
ITIME=12 

90 DEG -STARBOARD TURN 

CALL TURNCX,Y, TRATE,VEL,HDG,DT,ITIME,T,1) 
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GO TO 800 

ELSEIF(T(I).£Q.1500.) THEN 

LARGE 360 DEG STARBOARD TURN 

ITIME=48 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 
GO TO 800 

ELSEIFC(T(I).EQ.3000.) THEN 

SMALL 360 DEG STARBOARD TURN 

ITIME=24 

TRATECI)=1%PI180 

CALL TURNCX,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 
GO TO 800 

ELSEIF(T(I).EQ.4200.) THEN 

90 DEG STARBOARD TURN 

ITIME=12 

TRATECI)=0.5*PI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 
GO TO 800 

ELSEIF(T(I).EQ.5040.) THEN 

90 DEG STARBOARD TURN 

ITIME=12 

TRATECI)=0.5*PI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 
GO TO 800 

ELSEIF(T(I).EQ.5460.) THEN 

90 DEG STARBOARD TURN, BEGINNING OF SMALL S TURN 
ITIME=6 

TRATECI)=-1¥PI180 

CALL TURNCX,Y,TRATE,VEL,HDG,DT,ITIME,T,1I) 
GO TO 800 

ELSEIF(T(I).E£Q.5550.) THEN 

180 DEG STARBOARD TURN 

ITIME=12 

TRATECI)=1%PI180 

CALL TURNCX,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 
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GO TO 800 

ELSEIFCTCI).EQ.5760.) THEN 

180 DEG PORT TURN 

ITIME=12 

TRATECI) =-1*P1180 

CALL TURNC(X,Y,TRATE,VEL,HDG, DT,ITIME,T,1I) 
GO TO 800 

ELSEIFCTCI).E@.5940.) THEN 

90 DEG STARBOARD TURN, ENDING OF SMALL S TURN 
ITIME=6 

TRATECI)=1.0*%P1I180 

CALL TURNCX,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 
GO TO 800 

ELSEIFCTCI).EQ@.6540.) THEN 

90 DEG STARBOARD TURN, BEGINNING OF LARGE S TURN 
ITIME=12 

TRATECI)=0.5*P1180 

CALL TURNCX,Y,TRATE,VEL,HDG, DT, ITIME,T,I) 
GO TO 800 

ELSEIFCTCI) .EQ.6720.) THEN 

180 DEG PORT TURN 

ITIME=24 

TRATECI)=-0.5*PI180 

CALL TURNC(CX,Y,TRATE,VEL,HDG, DT,ITIME,T,I) 
GO TO 800 

ELSEIFCTCI).E@.71490.) THEN 

180 DEG STARBOARD TURN 

ITIME=24 

TRATECI)=0.5xPI180 

CALL TURNCX,Y,TRATE,VEL,HDG, DT, ITIME,T,I) 
GO TO 800 

ELSEIFCT(I).EQ.7500.) THEN 

90 DEG PORT TURN 

ITIME=12 

TRATECI)=-0.5*P1I180 
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800 


20 
1000 
100 
1500 


CALL TURNCX,Y, TRATE,VEL,HDG, DT,ITIME,T,1I) 
GO TO 800 

ELSEIF(TCI).EQ.8250.) THEN 

90 DEG STARBOARD TURN 

ITIME=1l2 

TRATECI)=0.5¥%PI180 

CALL TURNCX,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 
GO TO 800 

ENDIF 

TRATECI+1)=TRATECT). 

HDG(I+1)=HDGCT) 

VELCI+1)=VELCTI) 

VX=VELCIJXSINCHDGCI+1)) 
VY=VELCTI)*®COSCHDGCI+1)) 

XCI+1)=XCI)+VXXDT 

YCI#+1) =YCIV+VYXDT 

TCI+1)=TCI)+DT 

T=I+1] 

IFCI.LT.564) GO TO 34 

Ty =te) 

DO 20 J=1,11,4 

CONVERT HEADING FROM RADS TO DEGS. 
HDG(J)=HDGCJ)/PI180 

CONVERT VELOCITY FROM YARDS/SEC TO KTS 
VELCJ)=VELCJ)70. 5626 

CONVERT X AND Y TO NM 

XC J =XOII72025. 3667 

YCJ=YCIJ 972025. 3667 

TC J) =TCJ)760. 

WRITEC6,1500) TCJ),XCJ),YCJ),VELCJ),HDGCJ) 
WRITEC3,1500) TCJ),XCJ),YCJ),VELCJ),HDGCJ) 
CONTINUE 

FORMAT C€F&.2,2€4X%,F11.9),49X,F7.2,49X,F7.2) 
FORMAT C€F10.5,2X,F8.2) 

FORMAT (F&.2,49X,F11.4%4,4X,F11.4,4X,F11.4,4X,F7.2) 
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200 
2000 


mono Oo OM 


onaonanno0o0eaoeo0oreaoaneaonoeanoeneanaoneanaoooenonana na nnn na 7” © 


FORMAT (7,2X,'TIME SLOT = ',14) 
FORMAT (7,5X,F8.2,5X,F11.5,5X,F11.5) 


STOP 
END 


SUBROUTINE TURNCX,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 


RX PURPOSE xx 
CALCULATE X AND Y COMPONENTS IN TURNS 


¥¥X VARIABLE DEFINITIONS *xx 


DT 
HDG 


ISTOP 
ITIME 


TRATE 


TWOPI 
VEL 
VX 
VY 


SAMPLE INTERVAL IN SECS. 
MATRIX OF TARGET'S HEADINGS 
COUNTER 
TIME INTERVAL TURNSTOPS 
INTERVAL TO TIME TURNS,ITIME =12, 
EX. ITIME X DT X TRATE = DEGREES OF TURN 
12 X 15 SEC X 0.5 DEG/SEC = 90 DEGS 
COUNTER 
TIME USED IN SECONDS AND CONVERTED TO MINUTES FOR 
OUTPUT 
TURN RATE-INPUT IN DEG/SEC, CONVERTED TO RAD/SEC 
+ CLOCKWISE,- COUNTER CLOCKWISE 
2X PI 
MATRIX OF TARGET'S VELOCITIES-KNOTS AND YARDS/SEC 
X COMPONENT OF VEL 
Y COMPONENT OF VEL 
X COMPONENT-USED IN NM AND YDS 
Y COMPONENT-USED IN NM AND YDS 


¥¥*% VARIABLE DECLARATIONS xx 


10 


REAL HDG 

DIMENSION T(2000),VEL(2000) 

DIMENSION TRATE(2000),X(2000),Y(2000),HDG(2000) 
TWOPI=6 .2831853 

ISTOP=I+ITIME-1 

DO 10 J=I,ISTOP 

TRATE(J+1)=TRATECJ) 
HDG(J+1)=HDG( J )+TRATEC J) XDT 
IFCHDG(J+1).GT.TWOPI) HDG(J+1)=HDG(J+1)-TWOPI 
VELCJ+1)=VELCJ) 

VX=VEL (J) XSINCHDGCJ41)) 
VY=VEL (CJ) *COSCHDG(J41)) 

XC J41) =XC J) +4VXHDT 

YC J+1)=YCJ4VYDT 

TCJ41)=TCJ)+DT 

CONTINUE 

I=ISTOP 

RETURN 

END 


output from target track, 


file device 3 


time 
mins 
0.00 
1.00 
2.00 
3.00 
4.00 
5.00 
6.00 
7.00 
8.00 
9.00 
10.00 
11.00 
12.00 
13.00 
14.00 
15.00 
16.00 
17.00 
18.00 
19.00 
20.00 
21.00 
22.00 
23.00 
24.00 
25.00 


x comp 


nim 


10. 
10. 
10. 
10. 
10. 
10. 
10. 
10. 
.0000 
.0000 
.0000 
91 So 
-9115 
-8506 
7473 
-6640 
- 5806 
-4973 
-4140 
- 3306 
~24973 
- 1640 
0806 
<9919 
-9140 
-8306 


10 
10 


p— 
So 


owe mw OO hlUlUOOUlUlCOWUCOUCOClCCOUCOlClCUCOClCUOCOCD 


0000 
0000 


0000 


0000 
0000 
0000 
0000 
0000 


y comp 


nin 


12. 
Li 
-8333 
- 7500 
-6667 
- 5833 
. 5000 
-4167 
~ 3333 
-2500 
- 1667 
0886 
-0342 
.0181 
.0181 
.0181 
es 
Wh 
Pi. 
Ls 
ll. 
-0181 
-0181 
bh) bas 
| 
Lhe 


ll 
11 
ll 
ll 
ll 
ll 
ll 
ll 
11 
ll 
ll 
11 
11 
ll 


ll 
ll 


0000 
9167 


0181 
0181 
0181 
0181 
0181 


0181 
0181 
0181 
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input to tracking algorithm. 


velocity 


kts 


un un un un un nu un un nn un nm un um mu Mm wm um uM mM uM wu 


.0000 
.0000 
-0000 
.0000 
-0000 
-0000 
0000 
.0000 
.0000 
.0000 
.0000 
.0000 
.0000 
.0000 
.0000 
-0000 
.0000 
.0000 
.0000 
-0000 
-0000 
-0000 
.0000 
- 0000 
-0000 
.0000 


heading 


degs 
180.00. 
180.00 
180.00 
180.00 
180.00 
180.00 
180.00 
180.00 
180.00 


180.00 


180.00 
210.00 
240.00 
270.00 
270.00 
270.00 
270.00 
270.00 
270.00 
270.00 
270.00 
270.00 
270.00 
270.00 
270.00 
270.00 


_¢ sample sonobuoy pattern input file 

c for time t=0 to t= 59 

c this pattern was used in scenario 5 

¢c a priori information used for p(k/k-1) and x(k/k~-1) 
Cc 

c input data follows: 


c number of buoys in pattern 


c buoy bflag x comp y comp 
c type in nm in nm 
c 
3 
DIFAR 1.0 9.0 12.0 
LOFAR 2.0 8.0 10.0 
LOFAR 2.0 12.0 11.0 
c 
Cc 


c sample sonobuoy pattern input file 
c for time t=60 to t=140 
c using x(k/k) and P(k/k) from previous run 


c pattern and data is similar to that used in scenario 4% 


c input data follows: 


c number of buoys in pattern 


c buoy bflag x comp y comp 

Cc type in nm in nm 

¢ time 

c PCK/k) = p€59/59) is a 5 x 5 matrix 
C x(k7k) = x€59/59) is a l x 5 matrix 
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3 


DIFAR 1.0 9.0 12.0 

LOFAR 2.0 S6%0 10.0 

LOFAR 7 | 1 

5 .9000E+01 

1.7942E+09 2.0082E+03 3.9967E+02 3.868¢E+01 -5.9835E+00 
2.0082E+05 6.9507E+02 ~3.2991E+02 ~-1.2371E+02 -1.7275E+00 
3.9968E+02 ~3.2991E+02 7.5759E+03 G.3116E+02 7.8573E-01 
53.8682E+01 -1.2571E+02 4.3116E+02 1.5582E+02 ~1255E-01 


3 
-5.9835E+00 -1.7275E+00. 7.8573E-01 3.1255E-01 4%.8693E-03 
1.5318E+09 -1.43582E+02 2.2192E+09 -3.6561E+00 2.9992E+02 
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Cc 


Cc 


exec's used to run the tracking algorithm 


example of temp exec used to reserve space on disk b 
as indicated in buoy exec file device 7,8 10,12,18, and 19 


are on disk b. in order to see disk b type "flist * * b"” 


c temp exec 
CP DEFINE T3350 AS 193 20 
FORMAT 193 B 


¢ 

c example of buoy exec 

c filedef 2 is sonobouy pattern input file 

Cc filedef 3 is the target track input file 

c filedef 4 1s the graphic input for geographic and enlarged 
Cc geographic plots - 

c filedef 7 is the graphic input for the Kalman gains plots 
Cc filedef 8 is the output of the tracking algorithm. Each 

c matrices and calculation can be checked if desired. 

c filedef 9 is the graphic input for the variance plots 

€ filedef 10 is the track position data and the filter's estimated 
c Position output (Cin yards) 

c filedef 12 is the graphic input for the kalman position 

c variances and the experimental position variances. 

Cc filedef 18 is the graphic input for the predicted residual 
Cc and adaptive gating plots 

Cc filedef 19 is the error ellipsoid data 

c 

&TRACE ON 


CP TERMINAL LINES 80 
FILEDEF O02 DISK BUOY2 DATA A 
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FILEDEF 
FILEDEF 
FILEDEF 
FILEDEF 
FILEDEF 
FILEDEF 
FILEDEF 
FILEDEF 
FILEDEF 
FILEDEF 


03 
04 
07 
08 
09 
10 
12 
18 
19 
06 


LOAD BUOY 


DISK 
DISK 
DISK 
DISK 
DISK 
DISK 
DISK 
DISK 
DISK 
TERM 


TGT INPUT A 

FILE FTO4FO001 CPERM 
FILE FTO7FOO1] B 
OUTPUT DATA B 

FILE FTO9FO01 CPERM 
TRACK DATA B 

FILE FT12F001 B 
FILE FTI8FOO1 B 
ELLIP DATA B 


CSTART 
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¥¥X PURPOSE xxx 

THIS IS A MULTISENSOR TRACKING PROGRAM. PROGRAM USES EXTENDED 
KALMAN FILTER TECHNIQUES TO TRACK A MANEUVERING TARGET BASED 
ON NOISY PASSIVE BEARING AND DOPPLER SHIFTED FREQUENCY 


MEASUREMENTS. 


IN ORDER TO RUN PROGRAM SEE THE TWO EXEC FILES DEFINE 
ON THE PREVIOUS PAGES. TO RUN THE PROGRAM USE THE 
FOLLOWING STEPS. 

1. RESERVE SPACE ON B DISK BY USING TEMP EXEC. 

2. COMPILE THIS PROGRAM. 

3. EXECUTE BUOY EXEC. 


*%%% VARIABLE 


AFLAG 
BEAR 

BFLAG 
BMEAS 


BRG 
BRKKM1 
BIYPE 
D 
DBKKM1 
DBRG 
DOPP 
DSEED 
DT 

E 

EXT 


DEFINITIONS 3x 

= ADAPTIVE GATING FLAG; 0.0-OFF,1.0-ON. 

= SUBROUTINE TO CALC BEARING MEASUREMENT 

= DEFINES THE BUOY TYPE; 1.0- DIFAR, 2.0-LOFAR 

= SUBROUTINE TO CALC MEASUREMENT MATRIX HCK) 
FOR THE BEARING MEASUREMENT 

= BEARING IN RADS 

s PREDICTED BEARING MEAS. IN RADS, BRGCK/K-1) 

= BUOY TYPE, DIFAR OR LOFAR 

= DISTANCE OR RANGE, USED IN NM AND YARDS 

3 PREDICTED BEARING MEAS IN DEGS, DBRGCK/K-~-1) 

= BEARING IN DEGS 

= SUBROUTINE TO CALC DOPPLER FREQUENCY 

= NUMBER USED IN PSEUDO RANDOM NUMBER GENERATOR 

= TIME DIFFERENCE, TC(KJ- TCK-1) 

= PREDICTED RESIDUAL 

= ESTIMATED X COMP, XKK(C1) 
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EYT 

FD 
FDKKM1 
FDOPP 
FLAG 


FMEAS 


GAIN 
GAM 
GAMT 
GATE 
GATES 


GAUSS 


GE 
GFLAG 


HE 
HT 


INIT 


KK 
KOUNT 


LD 


MD 


ESTIMATED Y COMP, XKK(C3) 

ARRAY OF DOPP FREQS, ONE FOR EACH FREQ MEAS 
PREDICTED FREQ RESIDUAL FDCK/K-1) 

THE DOPP FREQ, OUTPUT OF DOPP SUBROUTINE 
INDICATES MORE MEASUREMENTS IN THE TIME INTERVAL 
1.0-REMAIN IN SAME TIME SLOT,0.0-CAN READ NEXT 
TIME 

SUBROUTINE TO CALC MEASUREMENT MATRIX HCK) 


* FOR FREQ MEASUREMENT. 


KALMAN GAINS 

SUBROUTINE TO CALC KALMAN EQUATIONS 

GAMMA MATRIX, STATE FORCING MATRIX (5 X 3) 
TRANSPOSE OF GAMMA 

HK) XPCK/K=1 HC K)T 

3 SIGMA ADAPTIVE GATE, 3%((GATE+R)*1/2) 
SUBROUTINE TO CALC GAUSSIAN PSEUDO-RANDOM 
NUMBER GENERATOR 

GRE 

USED WITH ADAPTIVE GATE, 0-CONTINUE 

1-GATE3 EXCEEDED, 2- REINITIALIZE PROBLEM 
MEASUREMENT MATRIX (5 X 1) 

A PRIORI HEADING INFORMATION 

TRANSPOSE OF H 

COUNTER 

INITIALIZATION FLAG TO DETERMINE WHICH P(0/-1) 
TO USE 

COUNTER 

ITERATION INTERVAL 

K-1, COUNTER 

COUNTER TO DETERMINE THE TIMES THE ADAPT GATE 
IS EXCEEDED 

ROW OR COLUMN OF MATRIX, 1 IN THIS CASE 
MAX NUMBER OF ROWS OR COLUMNS 

ROW OR COLUMNS OF MATRIX 

MAX NUMBER OF ROWS OR COLUMNS 
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MFLAG 
MZ 
MZZ 


NB 
NBUOY 
ND 
NFLAG 


NM 


NNZ 
NZ 
NZZ 
OFF 
PFLAG 


PHI 
PHIGAM 
PHIT 
PI 
PI180 
PKK 
PKKM1 
PLOTB 


PLOTF 


PLOTGB 


PLOTGF 


PROD 


QFIND 


USE MANEUVERING EQUATIONS, Q1(K) 
LAST TRACK VALUE OF DATA 

MZ-1 - 

ROWS OR COLUMNS OF MATRIX 

COUNTER FOR BUOY NUMBER 

TOTAL NUMBER OF BUOYS IN THE PATTERN 

MAX NUMBER OF ROWS AND COLUMNS 

INDICATES 0.0- NO NOISE IS ADDED TO 

MEAS, 1.0 NOISE IS INCLUDED 

USED WITH EXP VAR, 10 TERM MOVING WINDOW 
NM=K-10 

NZ + 10 

FIRST TRACK INTERVAL VALUE,USUALLY 1 OR 60 
NZ-1 

ERROR IN POSITION, IN YARDS 

0.0-CALC P(0/-1) FROM A PRIORI INFORMATION 
1.0-CALC P(K/K-1) FROM PREVIOUS SIMULATION 
TRANSITION MATRIX (5 X 5) 

SUBROUTINE TO CALC PHI AND GAMMA 

TRANSPOSE OF PHI 

3.1415927 

PI/180 DEGS 

P(K/K) COVARIANCE OF ERROR MATRIX 

P(K/K-1) PREDICTED COVARIANCE OF ERROR MATRIX 
SUBROUTINE TO PLOT BEARING MEAS. GAINS AND 
COV OF ERROR 

SUBROUTINE TO PLOT FREQ MEAS GAINS AND COV 
OF ERROR 

SUBROUTINE TO PLOT ADAPT GATE AND PREDICTED 
RESIDUAL FROM BEARING MEAS 

SUBROUTINE TO PLOT ADAPT GATE AND PREDICTED 
RESIDUAL FROM FREQ MEAS 

SUBROUTINE TO MULTIPLY MATRICES 

STATE EXCITATION MATRIX (5 X 5) 

SUBROUTINE TO CALC Q 
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RFLAG 


RFREQ 
RSTART 
SIGB 
SIGDG 
SIGF 
SIGFE 
SIGHE 
SIGMA 
SIGPOS 
SIGVE 
SIGVXE 
SIGVYE 
SUMX 


SUMY 


TEMP 
TEMP1 


TEMP2 
TEMPS 
TGATE 
THDG 
TIME 
TWOPI 
UNIT 
VARX 
VARY 
VE 
VMEAS 
VP 

VS 


MEAS NOISE COVARIANCE OF ERROR MATRIX 
CALCS THE NUMBER OF TIMES THE PROBLEM IS 
REINITIALIZED FOR A GIVEN MEASUREMENT 
TRUE RADIATED FREQUENCY 

SUBROUTINE TO REINITIALIZE THE PROBLEM 
STANDARD DEVIATION FOR BEARING MEAS NOISE,RADS 
STD DEV FOR BEARING MEAS NOISE, DEGS 

STD DEV FOR FREQ NOISE, HZ 

STD DEV FOR A PRIORI FREQ,HZ 

STD DEV FOR A PRIORI HEADING, DEGS 
VECTOR OF 3 STD DEVS FROM QFIND 

STD DEV FOR A PRIORI POSITION, NM 

STD DEV FOR A PRIORI VELOCITY, KTS 

STD DEV FOR VX COMP OF VE,KTS 

STD DEV FOR VY COMP OF VE,KTS 

SUMMATION FOR 10 TERM MOVING WINDOW FOR 
X COMP | 

SUMMATION FOR 10 TERM MOVING WINDOW FOR 
Y COMP 

MATRIX USED FOR CALCULATIONS 

MATRIX USED FOR CALCULATIONS 


MATRIX USED FOR CALCULATIONS 
MATRIX USED FOR CALCULATIONS 
COUNTS THE OF TIMES GATES IS EXCEEDED 
TRUE HEADING OF TARGET 

TIME OF MEAS 

2ePI 

IDENTITY MATRIX 

X COMP EXPERIMENTAL VARIANCE 

Y COMP EXPERIMENTAL VARIANCE 

A PRIORI VELOCITY INFORMATION 

MEAS VELOCITY IN DOPPLER EQUATION 
VELOCITY OF SOUND IN WATER, FT/SEC 
MATRIX OF THE VMEAS 
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VT = TARGET"'S TRUE VELOCITY 


vx =‘ TARGET'S TRUE X COMP VELOCITY 

VXE = A PRIORI X COMP VELOCITY INFORMATION 

VY = TARGET"S TRUE Y COMP VELOCITY 

VYE = A PRIORI Y COMP VELOCITY INFORMATION 

W = RANDOM FORCING FUNCTION COVARIANCE MATRIX 

XB = BUOY'S X COMP POSITION,NM AND YARDS 

XD = _ DISTANCE IN TARGET'S X COMP AND BUOY'S X COMP, 
XT-XB 

XDE = DISTANCE IN XE-XB 

XE = A PRIORI X COMP INFORMATION 

XKK = ESTIMATED STATE VECTOR, X(K/K) 

XKKM1 = PREDICTED STATE VECTOR, X(K/K-1) 

XT = TARGET*S TRUE X COMP 

YB = ~~ _BUOY'S Y COMP POSITION, NM AND YARDS 

YD = DISTANCE YT-YB 

YDE = DISTANCE YE-YB 

YE = A PRIORI Y COMP INFORMATION 

YT = TARGET'S TRUE Y.COMP 

z = MEAS MODEL 

ZDBRG = BEARING MEAS IN DEGS 

ZFREQ = MATRIX OF ZZFREQ'S 

ZGATE = SUBROUTINE TO CALC GATE3 AND TEST PREDICTED 
RESIDUAL 

ZZBRG = NOISY BEARING MEAS IN RADS 

ZZFREQ = NOISY FREQ MEAS IN HZ 


RRR VARIABLE DECLARATIONS xxx 


DIMENSION TIMEC200),XT(200),YTC200),VTC200), THDG( 200) 
DIMENSION XBC10),YBC10),BIYPEC10), BFLAGC10) 

DIMENSION VSC5,200),VX(200),VY(200) 

DIMENSION HC5,5),HTC5,5),605,5),PHI(5,5),TEMPC5,5),TEMP1C5,5) 
DIMENSION Q(5,5), PKK(C5,5),PKKM1(5,5),TEMP2(5,5),TEMP3(5,5) 
DIMENSION GAMC5,5),UNITC5,5),SIGMAC3) 
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DIMENSION 2(5),E(5),GEC5),WN05,5),EXT(C200),EYTC200) 
DIMENSION GAMTC5,5),PHITC5,5),XKKC5),XKKM1(5) 

DIMENSION ZDBRGC(5,200),DBRGC(5,200),FD(5,200),ZFREQC5, 200) 
REAL®8 DSEED 

CHARACTER®5 BTYPE 


INITIALIZE TERMS 

K=DISCRETE PT IN TIME, THE STAGE OF THE PROCESS 
L=1 ROW OR COLUMN 

M=2 ROW OR COLUMNS 

N=5 ROWS OR COLUMNS 

LD=MD=ND=MAX # OF ROWS OR COLUMNS 

DT=DELAY TIME 

MZ=NO. OF TRACK VALUES 


‘MZZ=MZ-1 USE IN DO LOOP 


NZ=1 

MZ=60 

PFLAG=0.0 
NZZ=NZ-1 : 
MZZ=MZ-1 
NNZ=NZ+10 

L=1 

M=3 

N=5 

LD=5 

MD=5 

ND=5 
PI180=0.0174533 
PT=3.1915927 
TWOPI=2PI 
RFREQ=300. 
VP=4860. 
FDOP=0. 
DSEED=50519 
FLAGS 
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NOISE FLAG-0.0 NOISE OFF,1.0 NOISE ON 

NFLAG=1.0 

MANV FLAG-0.0 MANV OFF,1.0 MANV ON 

MFLAG=1.0 

ADAPT GATING FLAG-0.0 OFF,1.0 ON 

AFLAG=1.0 

INITIALIZE FLAGS 

GFLAG=0. 

FLAG=0. 

NUMBER OF INITIAL PKKM1 MATRIX TO USE 

INIT=4 

INITIAL ESTIMATE OF TARGET POSITION, VELOCITY, AND COURSE IN 

NM, KTS, AND DEGS. 

IFCPFLAG.EQ.0.0) THEN 

XE=10. 

YE=12.0 

VE=5.0 

HE=180. 

STD. DEV. FOR PKKMC0O/7-1) 

POSIT=0.5NM,VEL=3KTS,HEADING=10DEG,FREQ=1 HZ 

SIGPOS=0.5 

SIGVE=3.0 

SIGHE=10 

SIGFE=1.0 

CONVERT XE AND YE TO YARDS 

XE=XE*%2025. 3667 

YE=YE¥2025.3667 

SIGPOS=SIGP0S¥2025.3667 

KTS TO YARDS/MIN 

VE=VE¥33.75635 

SIGVE=SIGVE*33.75633 

CALC VXE AND VYE OF THE TARGET 
VXE=VEXSINCHEXPI180) 
VYE=VExXCOSCHEXPI180) 
SIGVXE=SIGVEXSINC CHE+SIGHE) P1180) 
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oO 


On 7 


800 


C1001 


SIGVYE=SIGVEXCOS( CHE+SIGHE) ®P1180) 
ENDIF 
3 SIGMA GATE AND RESTART COUNTER FOR ADAPT. GATING 
GATE3=0. 
KOUNT=0 


CONVERT VP CVEL OF SOUND IN WATER) FROM FT/SEC TO YARDS/MIN 
VP=VP¥60.0/3.0 


READ IN ACTUAL TARGET DATA 


READ(3,1)(TIMECI), XTC1),YTCI),VTCI), THDGCI), I=NZ,MZ) 
FORMATCF8.2,4X,F11.4,4X,F11.4,4X,F11.4,4X,F7.2) 
WRITE ACTUAL TRACK VALUES 

WRITE(8, 1000) 

FORMAT(4X, "TIME", 12X, "XT",12X, "YT", 10X,"TGT VEL",6X,"TGT HDG") 
DO LOOP CONVERTS VT(I) FROM KTS TO OTHER UNITS 

DO 800 I=NZ,MZ 

CONVERT VT TO AGREE WITH VP 

KTS TO YARDS/MIN 

VICI) =VTC13¥33.75633 

CONVERT XT AND YT TO YARDS 

XTCI}=XTC1)¥2025. 3667 

YTC1)=YTCI)*2025. 3667 

CONTINUE 


WRITEC8,1)CTIMECI),XTCID,YTCI), VICI), THDGCI)D, I=NZ,MZ) 
FORMATCF8.2,309X,F11.9),9X,F7.2) 


READ IN NUMBER OF BUOYS, THEN READ TYPE AND LOCATION 
READC2,5) NBUOY 

FORMATCI3) 

BFLAG=1.0 BUOY GIVES BEARING AND FRE@ 
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1004 


1005 


1006 


oa nanan mn 


1008 


1009 


1010 


Cc 


BFLAG=2.0 BUOY GIVES FREQ ONLY 
READC4,6)CBTYPECI),XBCI),YBCI),I=1,NBUOY) 
READ(2,6)(BTYPECI), BFLAGCI) ,XBCI),YBCI),I=1,NBUOY) 
FORMAT (2X,A5,2X,F2.0,2X,F8.4,49X,F8.4) 

WRITE ACTUAL BUOYS -TYPES AND LOCATIONS 

CONVERT XB AND YB TO YARDS 

DO 1004 I=1,NBUO0Y 

XBCI)=XBCI)¥2025. 5667 

YBCI)=YBCI)#2025. 3667 

CONTINUE 

WRITEC8,1005) 

FORMAT(77,10X, "BUOYS') 

WRITEC8,1006) 

FORMAT( 3X, "TYPE", 3X, "FLAG*,8X, *XB*',13X, ‘YB*) 
WRITEC8,7) CBTYPECI), BFLAGCI),XBCI),YBCI),IT=1,NBUOY) 
FORMAT(C3X,A5,3X,F2.0,3X,F11.4,4X,F11.9%) 


CALC. THE ACTUAL BEARINGS FROM BUOYS TO TARGET TRACKS AND THE 
FREQS. RECEIVED 


‘WRITE BEARINGS AND FREQS FOR EACH BUOY 


DO 102 I=NZ,MZ 
WRITEC10,1008) 
FORMAT(7/,* POSIT °,4X,*XT°,11X,"YT') 
WRITEC10,1009) I,XTCI),YTCI) 
FORMATC14,2X,2C0F11.4%,2X)) 
WRITEC10,1010) 
FORMATC/,7X,"XB*,11X,"YB",7X, "BRG*,8X, "FREQ*) 
DO 1035 J=1,NBUOY 
XD=XTCI)-XBCJ) 
YD=YTCI)-YBCJ) 
CALL BEARCXD, YD, BRG) 
DBRGCJ,1I)=BRG/PI180 
CALC VX AND VY OF THE TARGET 
VXCIDSVTICID XSINCTHDGCI) *PI180) 
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1011 
103 
102 


899 


900 


25 
20 


VYCID=VTCI) XCOSCTHDGCI)*®PI180) 
CALL DOPPCVX,VY,XD,YD,VP,RFREQ, VMEAS, FDOP, I) 
- FDC J,I)=FDOP 
VSCI,J)=VMEAS 
WRITEC10,1010) XBCJ),YBCJ),XTCI)D,YTCI), DBRG(J,1),FDCJ,1) 
WRITEC10,1011) XBCJ),YBCJ),DBRGCJ,1I),FDCJ,I1) 
FORMAT(2(F11.4%,2X),F7.2,2X,F10.4) 
CONTINUE 
CONTINUE 
STARTS KALMAN FILTER PART OF PROGRAM 


K=NZ 
DO 899 I=1,M 
DO 899 J=1,M 
WCI,J)=0.0 
DO 900 I=1,N 
DO 900 J=1,N 
PHICI,J)=0. 
PHICI,1}3=1.0 
PKKM1(1I,J)3=0.0 
CONTINUE 
DO 20 I=1,N 
DO 25 J=1,M 
GAMCI,J)=0. 
CONTINUE 
CONTINUE 
IFCPFLAG.EQ.0.0) THEN 
XKKM1C01)=XE 
XKKM102)=VXE 
XKKM1(3)=YE 
XKKM109)=VYE 
XDE=XE-XBC1) 
YDE=YE-YBC1) 
CALL DOPPCVXE,VYE,XDE,YDE,VP,RFREQ, VMEAS,FDOP, 1) 
XKKM1¢05)=FDOP 
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G INITIAL PKKM1 MATRICES ARE SET UP USE 1 


IF CONTINUING FROM A PREVIOUS RUN PFLAG 


AND THE PKKM1 MATRIX WILL BE READ IN 
IFCINIT.EQ.1) THEN 

INITIAL #1 

PKKM1¢01,1)=1000. 

PKKM1¢02,2)=500. 

PKKM1(¢3,3)=1000. 

PKKM1(04,49)=500. 


ELSEIFCINIT.EQ.2) THEN 

INITIAL #2 

POSITION CINM)¥¥2, VEL. C2KTS) x2 
PKKM1(1,1)=9.0E6 

PKKM1(02,2)=4.0E3 

PKKM1(01,2)=1.49E4 
PKKM1¢2,1)=PKKM1(1,2) 
PKKM1(03,3)=4.0E6 

PKKM1¢03,4)=1.49E4 
PKKM1(04,3)=PKKM1(03, 49) 
PKKM104,49)=4.0E3 

ELSE 

INITIAL #3 

POSITION €.5NM)¥%2, VEL. C3KTS) 2 
PKKM1(1,1)=SIGPOS 
PKKM102,2)=SIGVXE 
PKKM1¢03,3)=SIGPOS 
PKKM1¢04,4)=SIGVYE 

ENDIF 


PKKM1(5,5)=SIGFE 
ELSE 

READC2,91) TIMECNZZ) 
DO 7235 I=1,N 
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=1.0 
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7325 


7177 


555 


3022 


8812 


91 
92 
67 


7905 


READ(2,*) (CPKKCI,J),J=1,N) 
WRITEC8,656) 

DO 7323 I=1,N 

WRITEC8,92) CPKKCI,J),J=1,N) 
READ(2Z,®) CXKKCI),I=1,N) 
WRITEC8,8011) 

WRITEC8,92) CXKK(CJ),J=1,N) 

GO TO 67 

ENDIF 


WRITEC8,7177) K 

FORMAT(//, "888888esee88 K=',19) 
WRITEC8,555) 

FORMAT (7 * PKKM1 MATRIX '*°) 

DO 3022 I=1,N 
WRITEC8,91) CPKKM1(I,J),J=1,N) 
WRITECS, 8812) 

FORMAT (7° XKKM1 *°) 
WRITEC8,91) CXKKM1(0J),J=1,N) 


FORMATC8C1PE12.4)) 
FORMAT(2X,8C1PE12.5,2X)) 

CONTINUE 

NB IS COUNTER INDICATING BUOY NUMBER 
NB=1 


IF(K.EQ.1) GO TO 3 
WRITE(8,7177) K 
DT=TIME(K)-TIME(K-1) 
WRITE(8,7905) DT 

FORMAT(5X,"  -DT=_ ‘*,F10.2) 
CALL PHIGAMCDT,N,M,PHI,GAM, K) 
WRITE PHI MATRIX 
WRITE(8,979) 
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3580 


978 
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3583 


2400 


656 


3023 


FORMAT(/! PHI MATRIX ') 
DO 3580 I=1,N 
WRITE(8,92) (PHICI,J),J=1,N) 
WRITE GAMMA MATRIX 
WRITE(8,978) 
FORMAT(“* GAMMA MATRIX ') 
DO 3581 I=1,N 
WRITEC8,92) (GAMC(I,J),J=1,™) 
CALL PRODCPHI,XKK,N,N,L,XKKM1,N,M,L) 
WRITEC8,8812) 
WRITEC8,92) (XKKM1(J),J=1,N) 
CALL QFIND(DT,GAM,XKKM1,W,Q,N,M,ND,MD, SIGMA, K,MFLAG, GFLAG) 
WRITEC38, 544) 
FORMAT(/' W*) 
DO 3021 I=1,3 
WRITE(8,92) (WCI,J),J=1,3) 
WRITEC8,799) 
FORMAT(/ Q MATRIX ') 
DO 3123 I=1,N 
WRITEC8,92) (QCI,J),J=1,N) 


IFCCBFLAGCNB) .EQ.1.0).0R.CBFLAGCNB) .EQ.2.0)) THEN 

RFLAG=0.0 

TGATE=0.0 

WRITEC8,35383) NB 

FORMAT(//,5X,*FREQ MEAS FROM BUOY *,12) 

CALL FMEASCXB, YB,XKKM1,VP,H,R,N,NB,K, FDKKM1,D) 

CALL GAINCPKK,PKKM1,Q,R,PHI,H,N,L,G,ND,MD,LD,K,FLAG, GATE, GFLAG, NZ) 
WRITEC8,2400) GATE 

FORMAT(/*GATE =*,1PE12.5) 


WRITEC8,656) 

FORMAT(7* PKK 2) 

DO 3023 I=1,N 

WRITEC8,92) (CPKKCI,J),J=1,N) 
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5009 


5110 


8811 


SOLVE XKK=XKKM1+GCK)CZC1)-FDKKM1 ) 


IF{GFLAG.EQ.1.) GO TO 5110 

CALC FREQ. CONFIDENCE LEVELS CIE STD DEV) 
D=D/2025. 3667 

IF(D.LE.2.) THEN 

SIGF=0.02 

SIGF=0.04 

ELSEIF(D.LE.5.0) THEN 

SIGF=0.04 

SIGF=0.06 

ELSEIF(CD.LE.10.0) THEN 

SIGF=0.08 

ELSE 

SIGF=0.1 

ENDIF 

WRITEC8,5009) SIGF 

FORMAT(/, "FREQ. STD DEV =',F4.2) 

ADD NOISE TO FREQ MEAS. 

CALL GAUSSCDSEED, SIGF,FDCNB,K),ZZFREQ, NFLAG) 
ZFREQ(NB,K)=ZZFREQ 


EC1)=ZFREQCNB, K)-FDKKM1 
WRITEC8,8811) 
WRITEC10,8811) 

FORMAT(7,8X,"Z",10X, "ZKKM1",8X, "ACTUAL *) 
WRITEC8,92) ZFREQCNB,K),FDKKM1,FDCNB,K) 
WRITEC10,92) ZFREQCNB,K),FDKKM1,FDCNB,K) 
WRITEC8, 3029) 

PRINT OUT ERROR 
WRITEC8,92) ECL) 
GFLAG=0. 


AFLAG=1.0 USE ADAPTIVE FILTER 
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3024 
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3124 


AFLAG=0. DON'T USE ADAPTIVE FILTER 
IFCAFLAG.EQ.1.) THEN 
DETERMINES IF RESIDUAL IS INSIDE GATE3 
GATE1=CH¥PKKM1 ¥H * +R) 0.5 
GATE3=3*GATE1] 
IFCK.NE.1) THEN 
CALL ZGATECEC1),GATE,R,W,GFLAG, KOUNT, GATE3) 
IFCGFLAG.EQ.1.0) TGATE=TGATE+1 .0 
IFCKOUNT.EQ.3) THEN 
CALL RSTARTCXB,XKKM1,PKKM1,RFLAG) 
GFLAG=2. 
RFLAG=RFLAG+1 .0 
WRITEC8,987) 
FORMAT (7 , © 3636363633838 RESTART THE PROBLEM 3€3€3€3€3€363€3€3€% * ) 
KOUNT=0 
WRITE XKKM1 © 
WRITEC8,8812) 
WRITEC8,92) CXKKM1CJ),J=1,N) 
WRITE PKKMI1 
WRITEC8, 555) 
DO 3024 I=1,N 
WRITEC8,92) CPKKM1(I,J),J=1,N) 
ENDIF 
IFCGFLAG.NE.0.) THEN 
CALL QFINDCDT,GAM, XKKM1,W,Q,N,M,ND,MD,SIGMA,K,MFLAG,GFLAG) 
WRITEC8, 549) 
544 FORMATC/* W ') 
DO 3025 I=1,3 
WRITEC8,92) CNCI,J),J=1,3) 
WRITEC8,799) 
DO 3124 I=1,N 
WRITEC8,92) (QCI,J),J=1,N) 
GFLAG=1. 
GO TO 8 
ENDIF 
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8011 
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3033 


ono oO © 


9899 


ENDIF 

IFCKOUNT.GT.0) KOUNT=KOUNT~-1 
ENDIF 

CALL PRODCG,E,N,L,L,GE,ND,MD) 
CALL ADDCXKKM1,GE,N,L,XKK,ND,MD) 
WRITEC8,8011) 

FORMAT(7* XKK *) 
WRITEC8, 92) CXKKCJ), J=1,N) 
WRITEC8,9897) K,NB 
FORMATC//* 333 SUMMARY FOR K= ',14,° FROM BUOY °,12, "RRR" ) 
WRITECS, 9899) 

EXTCK)=XKK(C1) 

EYT CK) =XKKC3) 

VARX=0.0 

VARY=0.0 

SUMX=0.0 

SUMY=0.0 

IFCK.GE.NNZ) THEN 

NM=K-10 

DO 3035 I=NM,K 
SUMX=SUMX*#CXTCI)-EXT CI) ) €¥2 
SUMY =SUMY+CYTCII-EYTCI) ) #2 

CONTINUE 

ENDIF 

VARX=SUMX/9. 

VARY=SUMY79. 


SET UP ARRAYS TO COLLECT GAINS, VARIANCES, AND ERROR ELLIPSOID 
DATA FOR PLOTS. 


CALL PLOTFCTIME, EXT, EYT,G,PKK,K,NB,NBUOY,NZ,MZ, VARX, VARY) 
STORE RESIDUAL AND GATES VS TIME FOR PLOTS 
CALL PLOTGFCTIME,K,NB,NBUOY,NZ,MZ,ABSCEC1)),GATE3, TGATE, RFLAG) 


FORMATC5X, *TIME', 9X, *XT*,11X, "YT", 12X, "EST XT*,11X,*EST YT") 
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4000 


4001 


4002 


3584 


WRITEC8,393) (CTIMECI),XTCID,YTCI),EXTCID,EYTCI), T=1,K) 
WRITEC8,393) TIMECK),XTCK),YTCK),EXTCK), EYTCK) 
WRITEC10,393) TIMECK),XTCK),YTCK),EXTCK),EYTCK) 
FORMAT(/,F8&8.2,4X,F11.3,4X,F11.3,4X,F11.5,4X,Fl1l1.3) 
ENDIF 


BEARING MEASUREMENT 


IFC CBFLAGCNB) .EQ.1.0).0R. (BFLAG(NB) .EQ.3.0)) THEN 
WRITE(38,3584) NB 

FCRMAT(//,5X, "BEARING MEAS. FROM BUOY ',12) 
IF(BFLAGCNB).EQ.1.0) THEN 

FLAG=1.0 
FLAG=1.0 MEANS REMAIN IN SAME TIME SLOT IE. K REMAINS THE SAME 


DO 4000 I=1,N 
XKKM1 CI) =XKKCI) 
WRITEC8,8812) 
WRITEC8,92) CXKKM1(J),J=1,N) 
DO 4001 I=1,N 
DO 4001 J=1,N 
PKKM1(I,J)=PKKCI,J) 
WRITEC8,555) 
DO 4002 I=1,N 
WRITEC8,92) CPKKMICI,J), J=1,N) 
ELSE 
FLAG=0.0 
ENDIF 
WRITEC8,5584) NB 
FORMAT(//,5X, "BEARING MEAS. FROM BUOY ‘',12) 
CALL BMEASCXB,YB,XKKM1,VP,H,R,N,NB,K, BRKKM1,D) 
RFLAG=0.0 
TGATE=0.0 
CALL GAINCPKK,PKKM1,@,R,PHI,H,N,L,G,ND,MD,LD,K, FLAG, GATE, GFLAG, NZ) 
WRITEC8,2400) GATE 


167 


4003 


5010 


5111 
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WRITEC8,656) 
DO 4003 I=1,N 
WRITEC8,92) CPKKCI,J),J=1,N) 


SOLVE XKK=XKKM1+G(K)(ZC1)-BRKKM1) 


IFCGFLAG.EQ.1.) GO TO 5111 
CALC BEARING CONFIDENCE LEVELS 
D=D/2025. 3667 
IFCD.LE.2.) THEN 
SIGDB=2.0 
SIGDB=5.0 
ELSEIFC(D.LE.5.0) THEN 
SIGDB=5.0 
SIGDB=10.0 
ELSEIFCD.LE.10.0) THEN 
SIGDB=10.0 
ELSE 
SIGDB=15.0 
ENDIF 
SIGB=SIGDBXPI180 
WRITEC8,5010) SIGDB 
FORMAT(/, "BEARING STD DEV = ',F4.2) 
ADD NOISE TO BRG MEAS. 
BRG=DBRGCNB,K)®PI180 
CALL GAUSS(DSEED,SIGB, BRG, ZZBRG,NFLAG) 
Z(1)=ZZBRG 
IF(BRKKM1.LT.0.) BRKKM1=BRKKM1+TWOPI 
EC1)=Z01)-BRKKM1 
IFCEC1).GT.PI) EC1)=EC1)-TWOPT 
IFCEC1L).LT.-PI) EC1)=EC1)+TWOPI 
WRITEC8, 8811) 
WRITEC10,8811) 
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95 
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3029 


3026 


WRITEC8,92) ZC1),BRKKM1,BRG 
DBKKM1=BRKKM1/PI180 
ZDBRGC(NB,K)=ZC1)/P1I180 
WRITEC8,93) ZDBRGCNB,K),DBKKMI, DBRGCNB,K) 
WRITEC10,94) ZDBRGCNB,K),DBKKM1, DBRGCNB,K) 
FORMAT(/,5X,F7.2,5X,F7.2,5X,F7.2) 
FORMAT(/,5X,F7.2,5X,F7.2,5X,F7.2) 

WRITEC8, 3029) 

FORMATC7 ! ERROR IE HEHE IE FE IE DE IE IE FE DE IE IE DE DE DE IE IE 3E 3E DE IE IE HEHEHE +) 

WRITEC8,92) EC1) 

GFLAG=0. 

AFLAG=1.0 USE ADAPTIVE FILTER 

AFLAG=0. DON'T USE ADAPTIVE FILTER 
IFCAFLAG.EQ.1.) THEN 

DETERMINES IF RESIDUAL IS INSIDE GATES 
GATE1=CH¥PKKM1¥H * +R) %%0.5 
GATE3=3*GATE1 

IF(K.NE.1) THEN 

CALL ZGATECEC1),GATE,R,W, GFLAG, KOUNT, GATES) 
IFCGFLAG.EQ.1.0) TGATE=TGATE+1.0 
IFCKOUNT.EQ.3) THEN 

CALL RSTARTCXB,XKKM1,PKKM1) 

GFLAG=2.0 

RFLAG=RFLAG+t+1. 

WRITECS8, 987) 

987 FORMAT C7, ° 3033 3HHH RESTART THE PROBLEM 3€3€3€3€3€3€ 33 3€3 * ) 
KOUNT=0 

WRITE XKKMI 

WRITECS8,8812) 

WRITEC8,92) CXKKMICJ),J=1,N) 

WRITE PKKM1 

WRITEC8, 555) 

DO 3026 I=1,N 

WRITEC8,92) (PKKMICI,J),J=1,N) 

ENDIF 
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3125 


3034 


IFCGFLAG.NE.0.) THEN 

CALL QFINDCDT,GAM, XKKM1,W,Q,N,M,ND,MD,SIGMA,K,MFLAG, GFLAG) 
WRITEC8, 549) 

544 FORMATC/° W') 

DO 3027 I=1,3 

WRITEC8,92) CWCI,J),J=1,3) 
WRITEC8,799) 

DO 3125 I=1,N 

WRITEC8,92) (QCI,J),J=1,N) 
GFLAG=1. 

GO TO 4 

ENDIF 

ENDIF 

IFCKOUNT.GT.0) KOUNT=KOUNT~-1 
ENDIF 

CALL PRODC(G,E,N,L,L,GE,ND,MD) 
CALL ADDCXKKM1,GE,N,L,XKK,ND,MD) 
PRINT OUT XKK 

WRITEC8,8011) 

WRITEC8,92) (CXKKCJ),J=1,N) 
WRITEC8,9897) K,NB 
WRITEC8,9899) 

EXT CK) =XKKO1) 
EYT CK) =XKK(3) 


VARX=0 .0 

VARY=0.0 

SUMX=0.0 

SUMY=0.0 

IFCK.GE.NNZ) THEN 

NM=K-10 

DO 3054 I=NM,K 
SUMX=SUMX#FCXTCII-EXTCI) ) #82 
SUMY=SUMY+CYTCI)D-EYTCI) ) ¥¥2 

CONTINUE 
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5000 


5001 


5002 


ENDIF 

VARX=SUMX/9. 

VARY=SUMY79. 

SET UP ARRAYS TO COLLECT GAINS, VARIANCES, AND ERROR ELLIPSOID 
DATA FOR PLOTS. 

COMMENT PLOTB OUT IF WANT ONLY VEL. ERROR ELLIPSES ONLY FROM 
PLOTF. 


CALL PLOTBCTIME, EXT, EYT,G,PKK,K,NB,NBUOY,NZ,MZ, BFLAG, VARX, VARY) 


STORE RESIDUAL AND GATES VS TIME FOR PLOTS 
CALL PLOTGBCTIME,K,NB,NBUOY,NZ,MZ,ABSCEC1)),GATES, TGATE, RFLAG) 


WRITEC8,35935) CTIMECI),XTCID,YTCI)D, EXTCID, EYTCI)D, T=NZ,K) 
WRITECS8, 393) TIMECK),XTCK),YTCK), EXTCK),EYTCK) 
WRITEC10,393) TIMECK),XTCK),YTCK), EXTCK),EYTCK) 
ENDIF 

IFCNB.LT.NBUOY) THEN 
NB=NB+1 

FLAG=1.0 

DO 5000 I=1,N 
XKKM1CI)=XKKCTI) 

WRITEC8,8812) 

WRITEC8,92) CXKKM1CJ),J=1,N) 

DO 5001 I=1,N 

DO 5001 J=1,N 

PKKM1CI,J)=PKKCI,J) 

WRITEC8,555) 

DO 5002 I=1,N 
WRITEC8,92) CPKKM1C(I,J), J=1,N) 


LOOP BACK TO CALC FREQ AND BEARING FROM NEXT BUOY 
GO TO 3 
ENDIF 
COMMENT OUT PLOTF AND PLOTB ABOVE. THE CALL BELOW WILL WRITE 
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888 


1015 


1014 
105 
104 


6000 


THE LAST ELLIPSE CALC. BY PLOTF OR PLOTB. 

IFCBFLAGCNB).EQ.2.) THEN 

CALL PLOTFCTIME, EXT, EYT,G, PKK, K,NB,NBUOY,NZ,MZ, VARX, VARY) 

ELSE 

CALL PLOTBCTIME, EXT, EYT,G,PKK,K,NB,NBUOY,NZ,MZ, BFLAG, VARX, VARY) 
ENDIF 

K=K+1 

KK=K-1 

IFCK.GT.MZ) GO TO 888 

GO TO 67 


SET UP FILES TO PLOT 
WRITEC(8,9897) KK,NB 
DO 104 I=NZ,MZ | 
WRITE( 10,1008) 
WRITEC10,1009) I,XTC(1),YTCI) 
WRITE(10,1013) 
FORMAT(/,6X, "XB", 9X, "YB", 9X, "BRG",6X, "ZDBRG', 7X, "FREQ", 7X, 


¥"ZFREQ*) 


DO 105 J=1,NBUOY 

WRITEC10,1014) XBCJ),YBC J), DBRGCJ,1),ZDBRGC(J,1),FDCJ,1),ZFREQCJ,1) 
FORMAT(2(F11.4%,2X),F7.2,2X,F7.2,2X,F10.4,2X,F10.9) 
CONTINUE 

CONTINUE 

WRITEC8, 9899) 

DO 6000 I=NZ,KK 
OFF=(CXTCID—-EXTCI) ) 8824+ CYTCID-EVTCI) ) #82) 880.5 
WRITEC8,393) TIMECI),XTCI)D,YTCI)D,EXTCI)D,EYTCI) 
WRITEC10,92) XTCI)D,YTCI)D,EXTC1I), EYTCI), OFF 

WRITEC4,92) XTCI)D,YTCI)D,EXTCID,EYTCI), OFF 

CONTINUE 

PRINT OUT GAINS, VARIANCES DATA FOR PLOTS 

CALL PLOTFCTIME, EXT, EYT,G,PKK,K,NB,NBUOY,NZ,MZ, VARX, VARY) 
PLOT THE RESIDUAL AND GATE3 VS TIME 

CALL PLOTGFCTIME,K,NB,NBUOY,NZ,MZ,EC1),GATE3,TGATE,RFLAG) 
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CALL PLOTBCTIME, EXT, EYT,G,PKK,K,NB, NBUOY,NZ,MZ,BFLAG, VARX, VARY 
PLOT THE RESIDUAL AND GATE3 VS TIME 

CALL PLOTGBCTIME,K,NB,NBUOY,NZ,MZ,E(1),GATE3, TGATE, RFLAG) 
WRITE(4,5) NBUOY 

WRITE(4,6001) (BFLAG(I),XBCI),YBCI),1=1,NBUOY) 
FORMAT(3X,F2.0,3X,F11.4,4X,F11.4) 

WRITE(4,5) INIT 

WRITE(5, 343) 

WRITE(2,91) TIMECKK) 

FORMAT(V' = PKK 

DO 3323 I=1,N . 

WRITE(2,91) (PKKCI,J),J=1,N) 

WRITE(2,91)(XKKCJ),J=1, ND 


STOP 
END 


SUBROUTINE BEAR (XD, YD, BRG) 
MX PURPOSE xx 

THIS GIVES ACTUAL BEARINGS FROM A BUOY TO THE TARGET IN RADIANS. 
USING NORTH AS 360 DEGS., EAST AS 90 DEGS, SOUTH AS 180 DEGS., 
WEST AS 270 DEGS. 


HH VARIABLE DEFINITIONS xx 
SAME AS MAIN PROGRAM 


PI180=0 .0174533 
PI=3.1415927 
TWOPT=6 .283185 
IFCYD.EQ.0.0) THEN 
IFCXD.GT.0.0) THEN 
BRG=90 .0¥PI180 
ELSE 
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BRG=270.0®P1I180 

ENDIF 
GO TO l 
ENDIF 
BRG=ATAN2(XD, YD) 
IFCBRG.LT.0.0) BRG=TWOPI+BRG 
RETURN 
END 


SUBROUTINE DOPP (VX,VY,XD,YD,VP,RFREQ, VMEAS,FD,I) 
%%X% PURPOSE 33€x 
SUBROUTINE CALCS. THE DOPPLER FREQ. 


¥%% VARIABLE DEFINITIONS xxx 
SAME AS MAIN PROGRAM, EXCEPT 
R = RANGECDISTANCE) 


¥%3% VARIABLE DECLARATIONS 3x 

DIMENSION VX(200),VYC(200) 

R= (XD*XD+YDXYD)%X0. 5 

VMEAS=CXDRVXCID+YDRHVYCI)DD/R 
FD=RFREQ/C1+CVMEAS/VP )) 

RETURN 

END 


SUBROUTINE PHIGAMCT,N,M,PHI,GAM,K) 
HHH PURPOSE 33x 
CALCULATE THE PHI AND GAMMA MATRICES 


44% VARIABLE DEFINITIONS 3x 
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SAME AS MAIN PROGRAM 


¥¥% VARIABLE DECLARATIONS 8x 
DIMENSION PHIC5,5),GAMC5,5) 


SET UP PHI MATRIX 
FORMAT(2X,8C1PE12.5,2X)) 
PHI(1,2)=T 
PHI¢3,4)=T 
GAMC1,1)=(TxT)72 
GAM(3,2)=GAM(1,1) 
GAM(2,1)=T 
GAM(4,2)=T 
GAM(5,3)=T 
REMOVE C'S TO GET PRINTOUT IF DESIRED 
WRITE(8,35) 
FORMAT(/,5X," PHI MATRIX ') 
DO 100 I=1,N | 
WRITEC8,92) (PHICI,J),J=1,N) 
WRITE(8,40) 
FORMAT(/,5X," GAMMA MATRIX ') 
DO 101 I=1,N 
WRITE(8,92) (GAMCI,J),J=1,M) 
RETURN 
END 


SUBROUTINE GAINCPKK, PKKM1,Q,R,PHI,H,N,L,G,ND,MD,LD,K,FLAG,GATE, 


¥GFLAG,NZ) 


HN PURPOSE 23x 
THIS SUBROUTINE COMPUTES THE OPTIMUM GAIN MATRIX AND THE 
COVARIANCE 
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RR¥X VARIABLE DEFINITIONS xxx 
SAME AS MAIN PROGRAM 


¥¥% VARIABLE DECLARATIONS xx3xx 

DIMENSION HC5,5),HIC5,5),G605,5),PHIC5,5),TEMP(5,5),TEMP1(5, 5) 
DIMENSION 9¢€5,5), PKKC5,5),PKKM1¢05, 5), TEMP2(5,5), TEMP3(5,5) 
DIMENSION GAM(5,5),UNITC5, 5) 

DIMENSION Z2(5),EC5),GEC5) 

DIMENSION GAMT(5,5),PHITC5,5),XKKC5),XKKM1(05) 


IFCK.EQ.NZ) THEN 
DO 900 I=1,N 
DO 900 J=1,N 
UNITCI,J)=0.0 
UNITCI,1)=1.0 
ENDIF 
REMAIN IN SAME TIME SO PHI AND GAM MATRICES ARE THE SAME 


- TFCCK.EQ.1).0R.CFLAG.EQ.1.0).OR.CGFLAG.EQ.1.0)) GO TO 8889 


NOTE HERE PKKMICI,J) = PCK/K-1) 
WHERE PCK/K-1)=PHIX¥PCK-1/K-1)*PHIT + Q 


CALC PKKMI1 


CALL TRANSCPHI,N,N,PHIT,ND,MD) 

CALL PRODCPKK,PHIT,N,N,N, TEMP,ND,MD,LD> 
CALL PRODCPHI, TEMP,N,N,N, TEMP1,ND,MD, LD) 
CALL ADD(TEMP1,9,N,N,PKKM1,ND,MD) 


CONTINUE 
IFCGFLAG.EQ.1.0) THEN 
CALL ADDCPKKM1,Q,N,N,PKKM1,ND,MD) 
ENDIF 
WRITEC8,8777) FLAG,GFLAG 
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WRITEC8,555) 
555 FORMAT(/* MATRIX PKKM1 *°) 
DO 3022 I=1,N 
3022 WRITEC8,92)CPKKM1CI,J),J=1,N) 
8777 FORMATC/*® FLAG= *,F4.2,2X,"'GFLAG =',F4.2) 


c CALC GAIN G(K) 

C 

c GK) = PCK/K-1)*HTXCHXPCK/K-1)*HT + R)30€-1 
CALL TRANSCH,L,N,HT,ND,MD) 
WRITE(8,39) 

39 -FORMATC' H') 
DO 22 I=l,L 


22 WRITEC8,92) CHCI,J),J=1,N) 
92 FORMAT (2X, 8C1PE12.5,2X)) 


C WRITECS, 36) 
36 FORMATC* HT ') 
C DO 23 I=1,N 


C25 WRITEC8,92) CHTCI,J),J=1,4L) 
CALL PRODCPKKM1,HT,N,N,1,TEMP,ND,MD,LD) 
WRITEC8,20) 
20 FORMATC* PKKM1L¥HT '*) 
DO 21 I=1,N 
a WRITEC8,92) CTEMPCI,J),J=1,L) 
CALL PRODCH,TEMP,L,N,L,TEMP1,ND,MD,LD) 
WRITEC8, 38) 
38 FORMATC' HP HT °) 
DO 50 I=1,L 
DO 50 J=1,L 
GATE=TEMP1CI, J) 
TEMP2CI,J)=TEMPI1CI,J) 
50 TEMP3CI,J=TEMPICI,J) 
DO 24 I=l1,L 
24 WRITEC8,92)¢(TEMPSCI,J),J=1,1L) 
TEMP3€1,1)=TEMP3C1,1)+R 
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3024 


30 
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37 


C45 


33 


35 


WRITEC8, 338) 
FORMATC® H P HT + R*) 
DO 224 I=1,L 
WRITECS8,92) CTEMP3CI,J),J=1,1L) 
WRITEC8, 31) 
FORMATC * CHPHT + R)-1") 
DET=1/TEMP3(1,1) 
DO 27 I=1,L 
WRITEC8,92) DET 
CALL CONSTCDET,TEMP,N,L,G,ND,LD) 
WRITEC8,99) 
FORMAT (7° MATRIX G '°) 
DO 3024 I=1,N 
WRITEC8,92)¢GCI,J),J=1,L) 
NOTE HERE PKKCI,J) = PCK/K) WHERE PCK/K) = 
CALL PRODCG,H,N,L,N,TEMP,ND,MD,LD) 
WRITECS8, 30) } 


CI-GCK) XH) XPCK/K~-1) 


FORMAT C * GH °) 


DO 25 I=1,N 
WRITECS,92)C(TEMPCI,J),J=1,N) 
WRITEC8,37) 

FORMATC'° IDENTITY MATRIX “) 
DO 45 I=1,N 

WRITEC8,92)€ UNITCI,J),J=1,N) 


CALL SUBCUNIT,TEMP,N,N,TEMP1,ND,MD) 
WRITEC8, 33) 

FORMATC*® I -GH *°) 

DO 35 I=1,N 
WRITEC8,92)(TEMPICI,J),J=1,ND) 
CALL PRODCTEMP1,PKKM1,N,N,N,PKK,ND,MD,LD) 
FLAG=0.0 
RETURN 
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END 


SUBROUTINE QFINDCDT, GAM, XKKM1,W,Q,N,M,ND,MD,SIGMA,K,MFLAG, GFLAG) 
%¥¥X% PURPOSE x 
CALCULATES THE STATE EXCITATION COVARIANCE OF ERROR MATRIX 


%%% VARIABLE DEFINITIONS xx 

SAME AS MAIN PROGRAM, EXCEPT 

SIGVT2=(€0.01 KTS/SEC)*x2 = 410.8 YDSRX2/MINHG 
SIGTH2Z=(€0.1DEG/SEC)¥¥2 = 0.01096 RADS¥X2/MINKRZ 
SIGFO2Z=(0.001HZ/SEC)¥X2= 0.0036 HZ¥¥2/MINHZ 
CALC W MATRIX WHERE W= ECWNCK) WS CK) ) 
WO1,1)=CSIGX) X¥2 

WO2, 2) =CSIGY ) ¥¥2 

WC1,2)=CSIGXY) 

WC3,3)=CSIGFO) ¥2=SIGFO2 


%%% VARIABLE DECLARATIONS 3x 
DIMENSION GAMC5,5),GAMTC(5,5),WC5,5),QC5,5),TEMPC5,5) 
DIMENSION XKKM1(5),SIGMAC3) 
SET UP W MATRIX 
IFCGFLAG.NE.1.) THEN 
IFCMFLAG.EQ.1) THEN 

MFLAG = 0 NO MANUEVERING 
MFLAG = 1 MANUEVERING 

SIGVT2 =410.8 

SIGTH2 = 0.01096 

SIGFO2 = 0.0036 
SIGMAC1)=SIGVT2 
SIGMAC2)=SIGTH2 
SIGMAC3)=SIGFO2 
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EVT2 = XKKM1C2) R¥2+XKKM1 CG) RZ 

WO1, LIZFCCXKKML C2) #2) EVT 2) XSIGVTI2+C XKKM1 C4) X¥2) XSIGTH2Z 
WO2,2)=CCXKKM1 C4) #¥2)/ EVT2) XSIGVT2+( XKKM1 C2) 82) XSIGTH2 
WO1,2)=C CXKKM1 02) XXKKM1 04) DZ EVT2) ®SIGVT2+CXKKM1 C2) XXKKM1 CG) ) & 
#SIGTH2 


WCO2,1)=WNC1,2) 
WC3,3)=SIGFO2 


ELSE 
WC1,1)=10. 
WC2,2)=10. 
WC3,3)=0.01 
ENDIF 
ENDIF 
543 WRITEC8, 5449) 
544 FORMATC/* W *) 
DO 3021 I=1,3 
3021 WRITEC8,92) CWCI,J),J=1,3) 
C 
CALL TRANSC(GAM,N,M,GAMT,ND,MD) 
C WRITEC8,1) 
1 FORMATC/,5X,* GAMT MATRIX ‘'*) 
C DO 100 I=1,™ 
C100 WRITEC8,92) CGAMTCI,J),J=1,N) 
92 FORMATC2X,8C1PE12.5,2X)) 
CALL PRODCGAM,W,N,M,M, TEMP,ND,MD,MD) 
C WRITECS, 2) 
2 FORMAT(/,5X,* TEMP MATRIX '*) 
C DO 101 I=1,N 
C101 WRITECS8,92) CTEMPCI,J),J=1,M) 
CALL PRODCTEMP,GAMT,N,M,N,Q,ND,MD,MD) 
C REMOVE C'S FOR PRINTOUT IF DESIRED 
C WRITECS, 3) 
3 FORMATC/,5X,* Q MATRIX °*) 
C DO 102 I=1,N 
C102 WRITEC8,92) (QCTI,J),J=1,N) 
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RETURN 
END 


SUBROUTINE FMEASCXB,YB,XKKM1,VP,H,R,N,NB,K, FDKKM1, D) 

HHH PURPOSE xx 

SUBROUTINE CALCS THE H MATRIX FOR THE FREQ. MEASUREMENTS 
AND SELECTS AND Re 


*¥¥%% VARIABLE DECLARATIONS Xxx 

DIMENSION XBC10),YBC10),XKKM1(5),H(5,5) 
FREQ NOISE STD DEV SFREQ=0.04HZ. 

SFREQ= 0.04 

R=SFREQHE2 

WRITEC8,40) 

FORMATC7*® R FOR FREQ *) 

WRITEC8,92) R 

WRITEC8,41) 

FORMAT(/* VP *) 

WRITEC8,92) VP 

FORMATC2X,8C1PE12.5,2X)) 

U=( CCXKKM101)-XBCNB) )XKKM1 02) )+€ CXKKM103)-YBCNB) ) ¥XKKM1 04) )) 
WRITECS8, 32) 

FORMAT (7° U *) 

WRITEC8,92) UV 

D=( CXKKM101)-XBCNB) )¥2+(XKKM1 03) -YBCNB ) ) 2) 0.5 
WRITEC8, 43) 

FORMAT (7 * D *) 

WRITEC8,92) D 

HO1,5)=1/701+CU/ CVPD) )) 
AK=C(XKKM1(05)¥CHC1 , 5) #2) )/ CVPXDRD) 
WRITECS8, 34) 

FORMAT (7 * AK *) 
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WRITEC8,92) AK 

HO1,1)=-AK¥C CXKKM1 (C2) 8D) -C UC XKKM1 01) -XBONB))7D)) 
HC1,2)=-AK¥D%( XKKM101)-XBCNB)) 

HCO1,3)=-AK%( CXKKM1 04) ¥D)-CUXCXKKM103)-YBCNB) 07D) 
HC 1,9) =-AK¥ DC XKKM103)-YBCNB) ) 

FDKKM1 =XKKM1(5)®HC1, 5) 

WRITEC8, 99) 

FORMAT Cs * FDKKM1 '") 

WRITEC8,92) FDKKM1 


RETURN 
END 


SUBROUTINE BMEASCXB, YB, XKKM1,VP,H,R,N,NB,K, BRKKM1 ) 
SUBROUTINE CALCS THE H MATRIX FOR THE BEARING MEASUREMENTS 


- AND SELECTS AND R. 


DIMENSION XBC10),YBC10), XKKM1(5),HC5,5) 
PI180=0.0174535 

BEARING NOISE STD DEV SDBRG=5 DEGS. 
SDBRG=5.0 

R=(SDBRG¥PI180 ) ¥¥2 

WRITEC8, 40) 

FORMATC/* R FOR BEAR. ‘'°) 
WRITEC8,92) R 

A=XKKM101)-XBCNB) 

B=XKKM1 03) -YBCNB) 

D=SQRTCAXH2Z+BRX2 ) 

FORMAT (2X, 8C1PE12.5,2X)) 

WRITECS8, 32) 
FORMATC/,5X,'A",12X, "B") 
WRITEC8,92) A,B 

WRITEC8,43) 
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FORMAT(’' D ') 
WRITE(8,92) D 
HC1,1)=B/ (DD) 
HC1,2)=0.0 

HC1,3)=-A/ (DED) 
H(1,4)=0.0 

HC1,5)=0.0 

CALL BEARCA,B, BRKKMI) 
WRITEC8, 44) 

FORMAT(/* BRKKMI ') 
WRITEC8,92) BRKKM1 
RETURN 

END 


SUBROUTINE PLOTFCTIME,XT,YT,G,PKK,K,NB,NBUOY,NZ,MZZ, VARX, VARY) 


HH PURPOSE XxX 


CONTAINS GRAPHIC DATA FOR KALMAN GAINS, COVARIANCE MATRIX, AND 


ERROR ELLIPSOIDS FOR FREQ MEAS 


HX VARIABLE DEFINITIONS xxx 
TERMS SAME AS MAIN PROGRAM, EXCEPT FOR 


ELLIP = SUBROUTINE TO CALC ERROR ELLIPSOIDS 

EVARX = MATRIX OF VARX 

EVARY = MATRIX OF VARY 

GF = MATRIX OF FREQ GAINS,STORE FOR PLOTTING 

GX = MATRIX OF X COMP GAINS, STORE FOR PLOTTING 
GXD = MATRICX OF VX COMP GAINS 

GY = MATRIX OF Y COMP GAINS, STORE FOR PLOTTING 
GYD = MATRIX OF VY COMP GAINS 

PF = MATRIX OF FREQ COMP COVARIANCE OF ERROR 
PVV = USED FOR PLOTTING ERROR ELLIPSOIDS 


PX 


X COMP VARIANCE, USED WITH ERROR ELLIPSOIDS 
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PXD = VX COMP VARIANCE, 


PXY = COVARIANCE OF X AND Y COMPS 
EY 2 Y COMP VARIANCE, USED WITH ERROR ELLIPSOIDS 
PYD = VY COMP VARIANCE 


*¥*X% VARIABLE DECLARATIONS 3x 

DIMENSION TIMEC200),XTC200),YT(200),GC5,5),PKKC5, 5) 

DIMENSION YHC120),XHC120),XP(200),YPC200) 

DIMENSION GXC6,200),GXDC6,200),GY(6,200),GYD(6,200),GFC6,200) 
DIMENSION PX(6,200),PXD(6,200),PY(6,200),PYD(6,200),PF(6,200) 
DIMENSION PXY(6,200)»PVV(6, 200), EVARX(6, 200), EVARY(6, 200) 
WRITEC7,1) NB 

WRITEC9,1) NB. 

FORMATC/,5X,* FREQ.MEAS. FROM BUOY °,I2) 

KK=K-1 

IFCK.GT.MZZ) GO TO 888 

GXC(NB,K)=GC1,1) 

GXDCNB, K)=GC2,1) 

GY CNB, K)=GC3,1) 

GYDCNB,K)=GC(4,1) 

GFCNB, K)=GC(5,1) 

SET UP PLOTS OF PKKM1 COMPONENTS 

PXCNB,K)=PKK(C1,1) 

PXDCNB,K)=PKK(2, 2) 

PYCNB,K)=PKKC3, 3) 

PYDCNB,K)=PKK(G, 4) 

PFCNB,K)=PKKC5, 5) 

PXYCNB,K)=PKK(1,3) 

PVV(NB,K)=PKK(2,4) 

EVARXCNB,K)=VARX 

EVARY CNB, K) =VARY 

PFLAG=0.0 

COMMENT OUT ONE OF CALLS IN EACH IF-THEN, THE 

FIRST CALL ELLIP COMPUTES POSITION ERROR ELLIPSES, SECOND ONE 
THE VELOCITY ERROR ELLIPSES 
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IF(K.EQ.1) THEN 
CALL ELLIPCXT,YT,PX,PY,PXY,K,NB, PFLAG) 
CALL ELLIPCXT,YT,PXD,PYD,PVV,K,NB,PFLAG) 
ELSEIF(K.EQ.11) THEN 

CALL ELLIP(XT,YT,PX,PY,PXY,K,NB, PFLAG) 
CALL ELLIPCXT,YT,PXD,PYD,PVV,K,NB, PFLAG) 
ELSEIF(K.EQ.31) THEN 

CALL ELLIP(XT,YT,PX,PY,PXY,K,NB, PFLAG) 
CALL ELLIPCXT, YT,PXD,PYD, PVV,K,NB, PFLAG) 
ELSEIF(K.EQ.61) THEN 

CALL ELLIPCXT, YT,PX,PY,PXY,K,NB, PFLAG) 
CALL ELLIP(XT,YT,PXD,PYD, PVV,K,NB, PFLAG) 
ENDIF 

GO TO 900 


DO 6001 I=1,NBUOY 

WRITE(7,80) I 

WRITE(9,81) I 

WRITEC19,81) I 

FORMAT(/,5X," FREQ MEAS. GAINS FROM BUOY ',I2) 

FORMAT(/,5X," FREQ MEAS. VAR. FROM BUOY ',I2) 

DO 6002 J=NZ,KK 

WRITEC7,95) TIMECJ),GXCI,J),GXDCI,J),GYCI,J),GYD(I,J),GFCI, J) 

WRITEC9,95) TIMECJ),PXCI,J),PXDCI,J),PYCI,J),PYD(I,J),PFCI,J) 

WRITEC19,95) TIMECJ),PXCI,J),PYCI,J),-PXYCI,J) 

IF(J.GE.11) THEN 

WRITEC12,95) TIMECJ) ,EVARXCI,J),EVARYC(I,J),PXCI,J),PYCI,J) 
ENDIF 

CONTINUE 

CONTINUE 

FORMATC(/F7 .2,2X, 5(1PE12.5,2X)) 
FORMATC(/,F8.2,4X,F11.3,49X,F11.3,49X,F1ll.3,4X,F11.3) 

RETURN 

END 
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SUBROUTINE PLOTBCTIME,XT,YT,G,PKK,K,NB,NBUOY,NZ,MZZ, BFLAG, VARX, 


XVARY) 


%%% PURPOSE 23x 
CONTAINS GRAPHIC DATA FOR KALMAN GAINS, COVARIANCE MATRIX,AND 
ERROR ELLIPSOIDS FOR BEARING MEAS 


%%% VARIABLE DEFINITIONS xx 
SAME AS MAIN PROGRAM AND/OR PLOTF 


¥%3 VARIABLE DECLARATIONS xx 
DIMENSION TIME(200),XT(200),YT(200),G(5,5),PKK(5,5) 
DIMENSION BFLAG(10),GF(6,200),PF(6,200) 

DIMENSION GX(6,200),GXD(6,200),GY(6,200),GYD(6,200) 
DIMENSION PX(6,200),PXD(6,200),PY(6,200),PYD(6,200) 
DIMENSION PXY(6,200),EVARX(6,200),EVARY(6,200) 
WRITEC7,1) NB 

WRITEC9,1) NB 

FORMAT(/,5X," BEARING MEAS. FROM BUOY ',12) 

KK=K-1 

IFCK.GT.MZZ) GO TO 888 

GX(NB,K)=GC1,1) 

GXDCNB,K)=G(2,1) 

GY(NB,K)=G(3,1) 

GYDC(NB,K)=G(4,1) 

GFC(NB,K)=G(5,1) 

SET UP PLOTS OF PKKM1 COMPONENTS 

PXCNB,K)=PKK(1,1) 

PXDCNB,K)=PKK(2, 2) 

PYCNB,K)=PKK(3, 3) 

PYDCNB,K)=PKK(4,4) 

PFCNB,K)=PKK(5, 5) 

PXYCNB,K)=PKK(1,3) 
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EVARX(NB,K) =VARX 

EVARYCNB,K)=VARY 

PFLAG=1.0 

IFCK.EQ.1) THEN 

CALL ELLIPCXT,YT,PX,PY,PXY,K,NB,PFLAG) 
ELSEIFCK.EQ.11) THEN 

CALL ELLIPCXT,YT,PX,PY,PXY,K,NB,PFLAG) 
ELSEIFCK.EQ.31) THEN 

CALL ELLIPCXT,YT,PX,PY,PXY,K,NB,PFLAG) 
ELSEIFCK.EQ.61) THEN 

CALL ELLIP(XT, YT, PX,PY,PXY,K, NB, PFLAG) 
ENDIF 

GO TO 900 


DO 6001 I=1,NBUOY 

DON,T PRINT GAINS AND VAR IF ITS A LOFAR BUOY CTHERE 0 ANYWAY) 
IFCBFLAGCI).EQ.2.) GO TO 6001 

WRITEC7,80) I 

WRITEC9,81) I 

WRITEC19,81) I 

FORMAT(7,5X,* BEARING MEAS. GAINS FROM BUOY ',12) 
FORMAT(/,5X,* BEARING MEAS. VAR. FROM BUOY *,I2) 

DO 6002 J=NZ,KK 

WRITEC7,95) TIMECJ),GXCI,J),GXDCI,J),GYCI,J),GYDCI,J),GFCI,J) 
WRITEC9,95) TIMECJ),PXCI,J),PXDCI,J),PYCI,J),PYDCI,J),PFCI,J) 
WRITEC19,95) TIMECJ),PXCI,J),PYCI,J),PXYCI,J) 

IFCJ.GE.11) THEN 

WRITEC12,95) TIMEC J), EVARXCI,J),EVARYCI,J),PXCI,J),PYCI,J) 
ENDIF 

CONTINUE 

CONTINUE 

FORMATC/F7 .2,2X,5C1PE12.5,2X)) 

RETURN 

END 
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SUBROUTINE ELLIPCXT,YT,P1,P3,P13,K,NB,PFLAG) 
%%% PURPOSE 3x 
%¥%% ROUTINE TO PLACE ELLIPSE DATA IN FILE 


4%% VARIABLE DECLARATIONS xx 
DIMENSION XT(200),YT(200),XP(200),YP(200) 
DIMENSION P1(6,200),P3(6,200),P13(6,200) 
A=2*P13(NB,K) 

B=P1(NB,K)-P3(NB,K) 
IFCCA.EQ.0.0).AND.(B.EQ.0.0)) B=0.0001 
THE1=. 50%ATAN2(A, BD 
A=(P1(NB,K)+P3(NB,K))/2. 

B=0.0 

IF(P13(NB,K).EQ.0.0) GO TO 10 
B=P13(NB,K)/SINC2.%THE1) 
SIG2X=(AtB) 

SIG2Y=(A-B) 

SX=( (SIG2X)**.5) 
SY=( (SIG2Y)**.5) 

PT=3.14159265/12 
CT=COS(THE1) 

ST=SINCTHEL) 

WRITEC4,9897) K,NB 

FORMAT(//* 2300 SUMMARY FOR K= ',14," FROM BUOY ',12, '%xx2x) 
IFCPFLAG.EQ.1.) THEN 
WRITEC4, 9898) 

FORMAT(/* BEARING MEAS. ERROR ELLIPSE *) 
ELSE 
WRITEC4,9999) 

FORMAT(/" FREQ. MEAS. ERROR ELLIPSE ') 
ENDIF 

DO 1981 IELLIP=1,25 
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XI=IELLIP 

XPCTELLIP) =SX¥COSCPT XI) XCT-SY¥SINCPT XID ¥ST+XT CK) 
YPC TELLIP)=SX¥COSCPTXXI)¥ST+SYXSINCPT¥XI)¥CT+HYTCK) 
WRITECG,1982)XPCIELLIP),YPCIELLIP) 

FORMAT (2F14.49) 

RETURN 

END 

¥¥% END OF ELLIPSE CALCULATION 


SUBROUTINE PLOTGFCTIME,K,NB,NBUOY,NZ,MZ,E,GATE3,  TGATE,RFLAG) 
¥¥% PURPOSE xx 

GRAHICS INPUT FILE ADAPT GATE AND PREDICTED 

RESIDUAL FROM FREQ MEAS 


¥¥% VARIABLE DEFINITIONS xxx 
SAME AS MAIN EXCEPT FOR 


ERR = MATRIX TO STORE PREDICTED RESIDUALS 
TRIP = MATRIX OF NUMBER OF TIMES GATES IS 
EXCCEDED 


¥%% VARIABLE DECLARATIONS 3x 
DIMENSION ERR(6,200),GATE(6,200), TIMEC 200) 
DIMENSION TRIPC5,200),RESTR(5, 200) 


IFCK.GT.MZ) GO TO 888 
ERRONB,KJ=E 
GATECNB,K) =GATES 
TRIPCNB,K)=TGATE 
RESTRCNB,K)=RFLAG 

GO TO 900 

DO 6000 I=1,NBUOY 
WRITEC18,80) I 
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FORMAT(/,5X,"FREQ. RESIDUAL AND GATE FROM BUOY ',I2) 
DO 6001 J=NZ,MZ 

WRITEC18,95) TIMECJ),TRIPCI,J),RESTRCI,J),ERRCI,J),GATECI,J) ~ 
CONTINUE 

CONTINUE © 

FORMATCF7 .2,2X,F5.2,2X,F5.2,2X,2C1PE12.5,2X)) 
RETURN 

END 


SUBROUTINE PLOTGBCTIME,K,NB,NBUOY,NZ,MZ,E,GATE3, TGATE,RFLAG) 
PLOTGB = SUBROUTINE TO PLOT ADAPT GATE AND PREDICTED 
RESIDUAL FROM BEARING MEAS 
GRAHICS INPUT FILE ADAPT GATE AND PREDICTED 

RESIDUAL FROM BEARING MEAS 


%%% VARIABLE DEFINITIONS xxx 
SAME AS MAIN EXCEPT FOR 


ERR = MATRIX TO STORE PREDICTED RESIDUALS 

TRIP = MATRIX OF NUMBER OF TIMES GATES IS 
EXCCEDED 

RESTR = MATRIX OF THE NUMBER OF RESTARTS FOR EACH 
MEAS 


%%% VARIABLE DECLARATIONS xx 
DIMENSION ERR(6,200),GATEC6,200),TIMEC 200) 
DIMENSION TRIPC5,200),RESTRC5, 200) 


IFCK.GT.MZ) GO TO 888 
ERRCNB,K)J=E 

GATECNB, K)=GATES 
TRIPCNB,K)=TGATE 
RESTROCNB,K)=RFLAG 
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GO TO 900 

DO 6000 I=1,NBUOY 

WRITEC18,80) I 

FORMATC/,5X,"BRG. RESIDUAL AND GATE FROM BUOY ‘,I2) 
DO 6001 J=NZ,MZ 

WRITEC18,95) TIMECJ),TRIPCI,J),RESTRCI,J),ERRCI,J),GATECI,J) 
CONTINUE 

CONTINUE 

FORMATCF7 .2,2X,F5.2,2X,F5.2,2X,2C1PE12.5,2X)) 
RETURN 

END 


SUBROUTINE ZGATECE,GATE,R,W,GFLAG, KOUNT,GATE3, TGATE) 
HH PURPOSE xxx 
CALCS THE GATES, AND RANDOM FORCING FUNC COV MATRIX VALUES 


¥%% VARIABLE DEFINITIONS ¥x% 
SAME AS MAIN PROGRAM, EXCEPT FOR 
GATE] = ONE SIGMA- (GATE + R) *x0.5 


4% VARIABLE DECLARATIONS xx 
DIMENSION W(5,5) 
GATE1=(GATE+R) 0.5 
GATE3=3XGATE1 

WRITE(8,96) GATES 

FORMAT(/, "GATES = ',1PE12.5) 
IFCABSCE)-GATE3.GT.0.) THEN 
GFLAG=1. 

TGATE =TGATE+1 

KOUNT=KOUNT+1 
WC1,1)=10.*NC(1,1) 
W(2,2)=10.*W(2, 2) 
W(3,3)=2*W(3, 3) 
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WRITEC3,97) 

FORMAT(C7,* GATES HAS BEEN EXCEEDED ') 
ENDIF 

RETURN 

END 


SUBROUTINE RSTARTCXB, XKKM1,PKKM1) 
4H PURPOSE 34363 
REINITIALIZES THE PROGRAM 


3% VARIABLE DEFINITONS 23x 
SAME AS MAIN PROGRAM 


433% VARIABLE DECLARATIONS 338% 
DIMENSION XKKM1(5),PKKM1(05,5),XBC10) 
DO 118 I=1,5 
DO 118 J=1,5 
PKKM1(1,J)=0.0 
PKKM1(1,1)=1.025E6 
PKKM1(03,3)=PKKM1(1,1) 
PKKM1(2,2)=1.025E4 
PKKM1 (04, 4) =PKKM1 (2, 2) 
PKKM1(5,5)=1.0 
RETURN 
END 


SUBROUTINE GAUSSCDSEED,SIG, MEAN, Z,NFLAG) 
%3H% PURPOSE 33x 
GAUSSIAN PSEUDO- RANDOM NUMBER GENERATOR 
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4% VARIABLE DECLARATIONS x 
DOUBLE PRECISION DSEED | 
REAL MEAN 

IFCNFLAG.EQ.1) THEN 

NR=1 

TEMP=0.0 

DO 10 I=1,12 

CALL GGUBS(CDSEED,NR,R) 

TEMP=TEMP+R . 

CONTINUE 

Z=(TEMP-6.0)¥SIG+MEAN 

ELSE 

Z=MEAN 

ENDIF 

WRITEC6,92) (MEAN,Z(1I),I1=1,NR) 
FORMAT(/,2X,"MEAN= *,F8.3,' Z= *,F8&.3) 

RETURN 

END 


SUBROUTINE GGUBS CDSEED,NR,R) 
MR PURPOSE HX 
BASIC UNIFORM (0,1) PSEUDO-RANDOM NUMBER GENERATOR 


¥¥% VARIABLE DECLARATIONS 33x 
INTEGER NR 

REAL RCNR) 

DOUBLE PRECISION DSEED 
SPECIFICATIONS FOR LOCAL VARIABLES 
INTEGER I 

DOUBLE PRECISION D2P31M,D2P 31 


193 


Qa no om 


© 


ono 7 © 


oO 


152 


D2P31M=(2"*31) - 1 
D2P31 =(2%*%31)COR AN ADJUSTED VALUE) 


DATA D2P31M/2147 483647 . DO/ 
DATA D2P31/2147483648 . DO/ 
FIRST EXECUTABLE STATEMENT 

DO 5 I=1,NR 


DSEED = DMOD( 16807 . DOXDSEED, D2P31M) 
RCI) = DSEED 7 D2P31 
RETURN 
END 


SUBROUTINE ADDCA,B,N,M,C,ND,MD) 
HHH PURPOSE XxX 
SUBROUTINE ADDS TWO MATRICES 
%%% VARIABLE DECLARATIONS xx 
DIMENSION ACND,MD),BCND,MD),CCND,MD) 


DO 152 I = 1,N 

DO 152 J = 1,M 

CCI,J) = ACI,J) + BCI,J) 
RETURN 

END 


SUBROUTINE SUBCA,B,N,M,C,ND,MD) 

HX PURPOSE xxx 

SUBROUTINE SUBTRACTS TWO MATRICES 

%%% VARIABLE DECLARATIONS x 
DIMENSION ACND,MD),BCND,MD),CCND, MD) 
DO 152 I = 1,N 
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DO 152 J = 1,™M 
C(I,J) = ACI,J) - BCI,J) 


RETURN 
END 


SUBROUTINE PRODCA,B,N,M,L,C,ND,MD,LD) 

HX PURPOSE 33x 

SUBROUTINE MULTIPLES TWO MATRICES 

%¥% VARIABLE DECLARATIONS 3x 
DIMENSION ACND,MD),BCMD,LD),CCND,LD) 


DO lI 
DO lJ 
C(I,J) 
DO 151 
DO 151 
DO 151 
C(I,J) 
RETURN 
END 


I 
J 
K 


I 
I 
0 


»N 
aL 


1,N 
1,L 
1, 


=C(I,J) + ACI, K)¥BCK, J) 


SUBROUTINE TRANSCA,N,M,C,ND,MD) 
HX PURPOSE 3363 
SUBROUTINE TRANSPOSES A MATRIX 
43% VARIABLE DECLARATIONS 3x 
DIMENSION ACND,MD),CCMD,ND) 


DO 153 
DO 155 
CCJ,I) 
RETURN 


I 
J 


1,N 
1,™M 


ACI, J) 
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100 


ll 
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120 


13 
14 


140 


15 


150 


END 


SUBROUTINE CONST(Q,A,N,M,C,ND,MD) 
HHH PURPOSE xxx 
SUBROUTINE MULTIPLES A MATRIX BY A CONSTANT 
* VARIABLE DECLARATIONS *%x 
DIMENSION ACND,MD),C(ND,MD) 
IF(Q) 11,10,11 _ 


DO 100 I = l, 

DO 100 J = l, 
C(I,J) = Q. 

RETURN 

IF (€Q-1.0) 13,12,15 


DO 120 I = 1,N 

DO 120 J = 1,™ 
C(I,J) = ACI,J) 
RETURN 

IF (€Q+1.0) 15,14,15 
DO 140 I = 1,N 

DO 140 J = 1,M 
CCI,J) = -ACI,J) 
RETURN 

DO 150 I = 1,N 

DO 150 J = 1,M 
CCI,J) = QRACI,J) 
RETURN 

END 
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SAMPLE OF GRAPHICS PROGRAM. 

¥%% PURPOSE xx 

THIS PROGRAM PLOTS THE TARGET'S ACTUAL TRACK, THE FILTER 
ESTIMATED TRACK, THE BUOY PATTERN AND THE ERROR ELLIPSOIDS 


xx VARIABLE DEFINITIONS %xx 
MAJORITY OF THE VARIABLES ARE SAME AS IN THE MAIN PROGRAM 


XP = ERROR ELLIPSOIDS X COMP 
YP = ERROR ELLIPSOIDS Y COMP 
MZZ = NUMBER OF TRACK POINTS 
NN = NUMBER OF ERROR ELLIPSOID POINTSC25 POINTS/ELLIPSE) 


DISSPLA TERMS ARE DEFINE IN THE DISSPLA BOOK 


%%% VARIABLE DECLARATIONS 3x 

REAL T(200),XT(200),YT(200), EXT (200), EYT (200), OF F(200) 
REAL XPC500),YPC500) 

REAL BFLAGC6),XBC6),YBC6) 


MZZ=60 
NN=525 
NE=NN725 
XORIG=10.0 
XMAX=25.0 
YORIG=15.0 
YMAX=30.0 
CALL TEK618 
CALL COMPRS 
CALL NOBRDR 
CALL PAGEC8.5,11.) 
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CALL 
CALL 
CALL 
CALL 
CALL 


HEIGHT(0.15) 
MXLALFC"'STANDART', '&") 
MX2ALFC'L/CSTD', '#") 
PHYSOR(2.0,2.0) 
AREA2D(6.0,6.0) 


GRAPHICS ROUTINE TO TRACK,BUOYS AND ERROR ELLIPSES 


CALL 


INTAXS 


CALL XNAMEC 'KYARDS$*,100) 


CALL 
CALL 


YNAMEC 'KYARDSS$*, 100) 
GRAF CXORIG, "SCALE", XMAX, YORIG, "SCALE*, YMAX) 


DO 10 I=1,NE 
DO ll J=1,25 
READ(C4,®) XPCJ),YPCJ) 
XP CJ) =XPCJ)71000.0 
YPC JV =YPCJ)71000 .0 
CONTINUE 
CALL DASH 
CALL CURVECXP,YP,25,0) 
CONTINUE 
READCG,®) (XTCID, YTCID,EXTCI), EYTCI), OF FCI), T=1,MZ2) 
READC4,%) NBUOY 
READ(4,%®) CBFLAGCI),XBCI),YBCI),I=1,NBUOY) 


DO 1 


T=1,M2Z2Z 


XTCI)=XTCI)71000.0 


YTCI)=YTCID71000.0 


EXTCI)=EXTCI)71000.0 
EYTCID=EYTCI)71000.0 
CONTINUE 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


HEIGHT(O.15) 
RESETC'DASH*) 
MARKER(C16) 
CURVECXT, YT,MZZ, 5) 
DASH 

MARKER(4 ) 
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1000 


CALL CURVECEXT, EYT,MZZ,5) 
CALL HEIGHT(0.15) 

DO 1000 I=1,NBUOY 
XBCI)=XBC(I)71000. 
YBCi)=YBCI)71000. 
TITX=XBCI)-1 

TITY=YBCI)+1 
IFCBFLAG(I).EQ.1.) THEN 
CALL RLMESS( "DIFAR$',100,TITX, TITY? 
CALL MARKER( 16) 

CALL SCLPIC(2) 

CALL CURVECXB(I),YBC(I),1,5) 
ENDIF 

IF(BFLAG(I).EQ.2.) THEN 
CALL RLMESS( *LOFAR$',100,TITX, TITY? 
CALL MARKER(16) 

CALL SCLPIC(2) 

CALL CURVECXB(I),YB(I),1,5) 
ENDIF 

CONTINUE 

CALL ENDPL(O> 

CALL DONEPL 

STOP 

END 
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HX PURPOSE xxx 

SAMPLE GRAPHICS PROGRAM PLOTS THE PREDICTED RESIDUAL, 
ADAPTIVE GATE, AND THE NUMBER OF TIMES THE GATE IS EXCEEDED 
FIRST THE PROGRAM PLOTS THE GRAPHS FOR THE FREQ MEAS, THEN ON 
ANOTHER PAGE PLOTS THE GRAPHS FOR THE BEARING MEAS. 

HENCE AS SHOWN IN FIG. 6-190 


*%% VARIABLE DEFINITIONS x 
SAME AS MAIN PROGRAM, AND STANDARD DISSPLA TERMS 


DRAW RESIDUAL AND GATE FOR FREQ MEASUREMENTS 

REAL 1(€500),ERRC500),GATEC500),TRIPC500),FLAGC500) 
XORIG=0.0 

XMAX=60.0 

XORIG=60.0 

XMAX=140.0 

YORIG=0.0 


‘YMAX=3.0 


CALL TEK618 

CALL COMPRS 

CALL NOBRDR 

CALL PAGEC11.,8.5) 

CALL HEIGHTCO.15) 

CALL MXI1ALFC'STANDART*, '&*) 

CALL MX2ALFC'L/CSTD*, * #*) 

READC4,*) CTCI), TRIPCI),FLAGCI), ERRCI),GATECI), 1=1,60) 
READCG,*) CTCID, TRIPCI), FLAGCI)D, ERRCI)D,GATECI), T=1,81) 
CALL PHYSORC.75,5.0) 

CALL AREA2D(4.0,2.75) 

CALL INTAXS 
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CALL XNAMEC*TIMECMINS)$*,100) 

CALL YNAMEC RESIDUAL & GATE MEAS$!*,100) 

CALL GRAFCXORIG, 'SCALE*,XMAX, YORIG, *SCALE', YMAX) 
CALL HEIGHT(O.15) 
CALL RLMESSC*BUOY 1¢9°,100,70.0,YMAX) 


CALL HEIGHT(O.15) 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


THKCRV (0.01) 
CURVEC(T,GATE,60,0) 
CURVECT,GATE,81,0) 
DASH 
CURVEC(T,ERR,60,0) 
CURVEC(T, ERR,81,0) 
RESETC*DASH® ) 
MARKER(C 2) 
CURVECT,TRIP,60,-1) 
CURVECT, TRIP,81,-1) 
RESETC *MARKER® ) 
ENDGR(0) 


XORIG=0.0 
XMAX=60.0 
XORIG=60 .0 
XMAX=140.0 
YORIG=0.0 
YMAX=3.0 


READC4,%) CTCI),TRIPCI),FLAGCID,ERRCID,GATECI), 1=1,60) 
READC4,%) (TCI), TRIPCI),FLAGCI),ERRCI),GATECI), 1=1,81) 


CALL 
CALL 


PHYSOR(6.5,5.0) 
AREA2D(4.0,2.75) 


CALL HEIGHT(0.15) 


CALL XNAMEC 'TIMECMINS)$*,100) 

CALL YNAMEC*RESIDUAL & GATE MEAS$'*,100) 

CALL GRAFCXORIG, *SCALE*,XMAX, YORIG, *SCALE*, YMAX) 
CALL HEIGHT(CO.15) 

CALL RLMESSC*BUOY 2$',100,70.,YMAX) 

CALL HEIGHT(O.15) 
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CALL THKCRV (0.01) 
CALL CURVECT,GATE,60,0) 
CALL CURVE(T,GATE,81,0) 
CALL DASH 
CALL CURVECT,ERR,60,0) 
CALL CURVECT,ERR,81,0) 
CALL RESET('DASH') 
CALL MARKER(2) 
CALL CURVE(T,TRIP,60,-1) 
CALL CURVECT, TRIP,81,-1) 
CALL RESETC'MARKER') 
CALL ENDGR(O) 
XORIG=0.0 
XMAX=60.0 
XORIG=60.0 
XMAX=140..0 
YORIG=0.0 
YMAX=3.0 
READCG,%) (TCI), TRIPCI), FLAGCI), ERR(I),GATE(I), 1=1,60) 
READ(4,%) (TCI), TRIPCI), FLAGCI), ERR(I),GATECI), I=1,81) 
CALL PHYSOR(.75,1.0) 
CALL AREA2D(4.0,2.75) 
CALL HEIGHT(0.15) 
CALL XNAMEC 'TIMECMINS)$", 100) 
CALL YNAMEC*RESIDUAL & GATE MEAS$",100) 
CALL GRAFCXORIG, "SCALE", XMAX, YORIG, "SCALE" » YMAX) 
CALL HEIGHT(O.15) 
CALL RLMESSC'BUOY 3$',100,70.,YMAX) 
CALL HEIGHT(0.15) 
CALL LINESP (2.0) 
CALL LINES ('FREQ GATE$', IPAK,1) 
CALL LINES ('RESIDUAL$", IPAK,2) 
XW=XLEGND CIPAK,2) 
YW=YLEGND CIPAK,2) 
CALL LEGLIN 
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CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


CALL 


THKCRV (0.01) 
CURVE(T,GATE,60,0) 
CURVE(T, GATE, 81,0) 
DASH 

CURVE(T,ERR»60,0) 
CURVE(T,ERR,81,0) 
RESETC "DASH? ) 
MARKER(2) 

CURVE(T, TRIP,60,-1) 
CURVE(T,TRIP,81,-1) 
RESET( "MARKER®) 
LEGENDCIPAK,2,6.,2.25) 


ENDPL(0) 


XORIG=0.0 
XMAX=60.0 
XORIG=60.0 
XMAX=140.0 
YORIG=0.0 
YMAX=1.0 


CALL 
CALL 
CALL 


NOBRDR 
PAGEC(11.,8.5) 
HEIGHT( 0.15) 


READCG,#®) CTCID,TRIPCID,FLAGCI),ERRCI),GATECI),1=1,60) 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


PHYSOR( .75,5.0) 

AREA2D(4.0,2.75) 

INTAXS 

XNAMEC *TIMECMINS)$*,100) 

YNAMEC*RESIDUAL & GATE MEAS$'*,100) 
GRAFCXORIG, "SCALE, XMAX, YORIG, "SCALE", YMAX) 


CALL HEIGHT( 0.15) 
CALL RLMESSC*BUOY 19',100,70.0,YMAX) 
CALL HEIGHT(0.15) 


CALL 
CALL 


THKCRV (0.01) 
CURVECT,GATE,60,0) 
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CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
PLOT 
PLOT 


CURVECT,GATE,81,0) 
DASH 
CURVECT,ERR,60,0) 
CURVECT, ERR, 81,0) 
RESETC*DASH'® ) 
MARKERC2) 

CURVECT, TRIP,60,-1) 
CURVECT, TRIP,81,-1) 
RESETC 'MARKER'® ) 
ENDGR(C0) 

FOR BUOY 2 

NEXT GRAPH 


XORIG=0 .0 
XMAX=60. 
XORIG=60.0 


XMAX= 


140.0 


YORIG=0.0 
YMAX=1.0 

READC4,%) CTCID,TRIPCI)D, FLAGCI)D, ERRCID,GATECI), 1=1,60) 
READ(G,%) (TCI), TRIPCI), FLAGCI), ERRCI),GATECI), I=1,81) 


CALL 
CALL 


PHYSOR(C6.5,5.0) 
AREA2D(4.0,2.75) 


CALL HEIGHT(O.15) 
CALL XNAMEC 'TIMECMINS)$*,100) 
CALL YNAMEC'RESIDUAL & GATE MEAS$',100) 
CALL GRAFCXORIG, 'SCALE', XMAX, YORIG, "SCALE' , YMAX) 
CAzL HEIGHT(0.15) 
CALL RLMESSC*BUOY 29°,100,10.,YMAX) 
CALL RLMESSC'BUOY 29°,100,70.,YMAX) 
CALL HEIGHT(0O.15) 


CALL 
CALL 
CALL 
CALL 
CALL 


THKCRV (0.01) 
CURVECT,GATE,60,0) 
CURVECT, GATE, 81,0) 
DASH 

CURVECT, ERR,60,0) 


CALL CURVE(T,ERR,81,0) 

CALL RESET('DASH') 

CALL MARKER(2) 

CALL CURVE(T, TRIP,60,-1) 

CALL CURVE(T,TRIP,81,-1) 

CALL RESET( 'MARKER') 

CALL LEGENDCIPAK,2,1.,2.25) 

CALL ENDGR(0) 

PLOT FOR BUOY 3 

PLOT NEXT GRAPH 

XORIG=0.0 

XMAX=60. 

XORIG=60.0 

XMAX=140.0 

YORIG=0.0 

YMAX=1.0 

READ(4,%) (TCI), TRIPCI), FLAGCI), ERR(I),GATECI),I=1,60) 
READ(4,%) (TCI), TRIPCI), FLAGCI), ERRCI),GATECI), I=1,81) 
CALL PHYSOR(.75,1.0) 

CALL AREA2D(4.0,2.75) 

CALL INTAXS 

CALL HEIGHT(0.15) 

CALL XNAME( "TIMEC(MINS)$",100) 

CALL YNAMEC "RESIDUAL & GATE MEAS$',100) 
CALL CROSS 

CALL GRAF(XORIG, "SCALE", XMAX, YORIG, "SCALE®, YMAX) 
CALL HEIGHT(0.15) 

CALL RLMESS( "BUOY 3$*,100,10.,YMAX) 
CALL RLMESS('BUOY 3$*,100,70.,YMAX) 
CALL HEIGHT(0.15) 

CALL LINESP (2.0) 

CALL LINES ('BRG GATE$', IPAK, 1) 

CALL LINES ('*RESIDUAL$', IPAK,2) 

CALL LINES ('EXCEED$', IPAK, 3) 

XW=XLEGND CIPAK,2) 
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YW=YLEGND CIPAK, 2) 

CALL MYLEGN ("BUOY 3$',100) 
CALL LEGLIN 

CALL THKCRV (0.01) 

CALL CURVE(T,GATE,60,TRIP) 
CALL CURVE(T,GATE,81,0) 
CALL DASH 

CALL CURVECT, ERR, 60,0) 

CALL CURVECT,ERR,81,0) 

CALL RESET('DASH') 

CALL MARKER(2) 

CALL CURVEC(T,TRIP,60,-1) 
CALL CURVECT, TRIP,81,-1) 
CALL RESET( 'MARKER') 

CALL LEGEND(IPAK,2,6.,2.25) 


CALL ENDPL(0) 
CALL DONEPL 
STOP 

END 
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